使用位姿图gtsam优化
步骤:
1.构建问题(因子图)
- 建立因子图模型
gtsam::NonlinearFactorGraph::shared_ptr graph(new gtsam::NonlinearFactorGraph);
- 初始化值
gtsam::Values::shared_ptr initial ( new gtsam::Values );
- 向initial中加入顶点(初始化顶点值)(gtsam中的Pose3):包括序号和对应类型的值
gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], data[5] );
gtsam::Point3 t ( data[0], data[1], data[2] );
initial->insert ( id, gtsam::Pose3 ( R,t ) );
- 添加边(对应到因子图中的因子):构造因子,向图中添加因子
构造因子所需参数:
连接顶点类型;
连接顶点的序号;
测量值;
高斯噪声模型,使用gtsam定义的信息矩阵形式构造
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information ( mgtsam );
gtsam::NonlinearFactor::shared_ptr factor (new gtsam::BetweenFactor<gtsam::Pose3> ( id1, id2, gtsam::Pose3 ( R,t ), model ) );
graph->push_back ( factor );
固定第一个顶点,在gtsam中相当于添加一个先验因子
gtsam::NonlinearFactorGraph graphWithPrior = *graph;
gtsam::noiseModel::Diagonal::shared_ptr priorModel = gtsam::noiseModel::Diagonal::Variances (
( gtsam::Vector ( 6 ) <<1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6 ).finished()
);
gtsam::Key firstKey = 0;
for ( const gtsam::Values::ConstKeyValuePair& key_value: *initial )
{
cout<<"Adding prior to g2o file "<<endl;
graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> (
key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel )
);
break;
}
2.问题求解
gtsam::LevenbergMarquardtParams params_lm;
params_lm.setVerbosity("ERROR");
params_lm.setMaxIterations(20);
params_lm.setLinearSolverType("MULTIFRONTAL_QR");
gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, params_lm );
gtsam::Values result = optimizer_LM.optimize();
3.提取优化结果
- 顶点使用result访问:
result[i].key;result[i].value.cast<类型>
for ( const gtsam::Values::ConstKeyValuePair& key_value: result )
{
gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
gtsam::Point3 p = pose.translation();
gtsam::Quaternion q = pose.rotation().toQuaternion();
}
- 边(因子)使用factor从图*graph中访问
for ( gtsam::NonlinearFactor::shared_ptr factor: *graph )
{
gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>( factor );
if ( f )
{
gtsam::SharedNoiseModel model = f->noiseModel();
gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = dynamic_pointer_cast<gtsam::noiseModel::Gaussian>( model );
if ( gaussianModel )
{
gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
gtsam::Pose3 pose = f->measured();
gtsam::Point3 p = pose.translation();
gtsam::Quaternion q = pose.rotation().toQuaternion();
gtsam::Matrix infoG2o = gtsam::I_6x6;
infoG2o.block(0,0,3,3) = info.block(3,3,3,3);
infoG2o.block(3,3,3,3) = info.block(0,0,3,3);
infoG2o.block(0,3,3,3) = info.block(0,3,3,3);
infoG2o.block(3,0,3,3) = info.block(3,0,3,3);
}
}
}