/home/w/
C++_test/gtsam_learn/src/RangeLocalization.cpp
: In function ‘int main()’
: /home/w/
C++_test/gtsam_learn/src/RangeLocalization.cpp
:66
:9
: error
: no matching function for call to ‘gtsam
::RangeFactor<gtsam
::Point2, gtsam
::Point2>
::RangeFactor(gtsam
::Key&, __gnu_cxx
::__alloc_traits<std
::allocator<gtsam
::Point2>, gtsam
::Point2>
::value_type&, __gnu_cxx
::__alloc_traits<std
::allocator<double>, double>
::value_type&,
boost::shared_ptr<gtsam
::noiseModel
::Isotropic>&)’ 66 | ));#include <gtsam/
geometry/Point2.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/
geometry/Pose2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h> // 备用优化器
#include <iostream>
#include <vector>
#include <random>
#include <cmath>
using namespace std;
using namespace gtsam;
int main() {
cout << "GTSAM 二维距离定位
示例 (
使用内置因子)" << endl;
// 1. 设置信标位置和真实位置
vector<Point2> beacons = {
Point2(2.0, 3.0),
Point2(-1.0, 4.0),
Point2(3.0, -2.0),
Point2(-4.0, -3.0)
};
const Point2 true_position(1.5, 2.5); // 真实位置
cout << "真实位置
: (" << true_position.x() << ", " << true_position.y() << ")\n";
// 2. 生成带噪声的测量距离
vector<double> measurements;
default_random_engine generator;
normal_distribution<double> noise(0.0, 0.1); // 高斯噪声 (均值0, 标准差0.1)
for (const auto& beacon
: beacons) {
double true_
distance = true_position.dist(beacon);
double measured_
distance = true_
distance + noise(generator);
measurements.push_back(measured_
distance);
cout << "信标 (" << beacon.x() << ", " << beacon.y()
<< ") 的真实距离
: " << true_
distance
<< ", 测量距离
: " << measured_
distance << endl;
}
// 3. 设置GTSAM优化问题
// 创建因子图
NonlinearFactorGraph graph;
// 创建键 (位置变量)
Key positionKey = Symbol('x', 0);
// 噪声模型 (0.1米标准差)
auto noiseModel = noiseModel
::Isotropic
::Sigma(1, 0.1);
// 添加距离因子到因子图（
使用内置RangeFactor）
for (size_t i = 0; i < beacons.size(); ++i) {
// 添加距离因子 - RangeFactor<Point2, Point2> 表示两点之间的距离
graph.add(RangeFactor<Point2, Point2>(
positionKey,
beacons[i], // 信标作为已知点直接传递
measurements[i],
noiseModel
));
}
// 4. 设置初始值 (
使用更好的初始估计)
Values initialEstimate;
Point2 initial_position(1.0, 1.5); // 比(0,0)更好的初始估计
initialEstimate.insert(positionKey, initial_position);
cout << "\n初始估计位置
: (" << initial_position.x() << ", "
<< initial_position.y() << ")" << endl;
// 5. 配置求解器选项
LevenbergMarquardtParams params;
params.setVerbosityLM("SUMMARY");
params.maxIterations = 100;
params.absoluteErrorTol = 1e-5; // 绝对误差容忍度
params.relativeErrorTol = 1e-5; // 相对误差容忍度
params.lambdaInitial = 1e-3; // 更小的初始 lambda
params.lambdaFactor = 2.0; // 减少 lambda 增长速率
// 6. 运行优化
cout << "\n开始优化..." << endl;
cout << "初始误差
: " << graph.error(initialEstimate) << endl;
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
Values
result = optimizer.optimize();
// 7. 输出结果
cout << "\n优化完成!" << endl;
cout << "迭代次数
: " << optimizer.iterations() << endl;
cout << "最终误差
: " << graph.error(
result) << endl;
Point2 estimated_position =
result.at<Point2>(positionKey);
cout << "\n优化后位置
: (" << estimated_position.x() << ", "
<< estimated_position.y() << ")" << endl;
cout << "真实位置
: (" << true_position.x() << ", " << true_position.y() << ")" << endl;
double position_error = estimated_position.dist(true_position);
cout << "位置误差
: " << position_error << "米" << endl;
// 计算并输出每个信标的残差
cout << "\n各信标残差分析
:" << endl;
for (size_t i = 0; i < beacons.size(); ++i) {
double predicted_
distance = estimated_position.dist(beacons[i]);
double residual = predicted_
distance - measurements[i];
cout << "信标 " << (i+1) << "
: 测量值 = " << measurements[i]
<< ", 预测值 = " << predicted_
distance
<< ", 残差 = " << residual << endl;
}
return 0;
}
本文介绍了一个使用Boost.Geometry库中default_distance_result计算二维点间欧几里德距离的C++程序。通过包含必要头文件、定义点结构体、编写计算距离函数，展示了如何利用boost::geometry::distance和default_distance_result模板类进行几何计算。
订阅专栏 解锁全文
扫一扫
545
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
