gtsam代码阅读

使用位姿图gtsam优化

步骤:

1.构建问题(因子图)

1. 建立因子图模型

gtsam::NonlinearFactorGraph::shared_ptr graph(new gtsam::NonlinearFactorGraph);

2. 初始化值

gtsam::Values::shared_ptr initial ( new gtsam::Values );

3. 向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 ) );

4. 添加边(对应到因子图中的因子):构造因子,向图中添加因子
   构造因子所需参数:
   　连接顶点类型;
   　连接顶点的序号;
   　测量值;
   　高斯噪声模型,使用gtsam定义的信息矩阵形式构造

gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information ( mgtsam );//使用信息矩阵建立高斯噪声模型,其中mgtsam为信息矩阵为原始信息矩阵沿斜对角线对称
gtsam::NonlinearFactor::shared_ptr factor (new gtsam::BetweenFactor<gtsam::Pose3> ( id1, id2, gtsam::Pose3 ( R,t ), model ) );//添加一个因子,betweenfactor<顶点类型>(序号1,序号2,观测值,噪声模型)
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.问题求解

// 我们使用 LM 优化
    gtsam::LevenbergMarquardtParams params_lm;//构建LM算法参数类(相当于g2o options)
    params_lm.setVerbosity("ERROR");//设置输出信息
    params_lm.setMaxIterations(20);//最大迭代次数
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");//分解算法
    gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, params_lm );//构建下降算法(图,初值,参数)
    
    // 你可以尝试下 GN
    // gtsam::GaussNewtonParams params_gn;
    // params_gn.setVerbosity("ERROR");
    // params_gn.setMaxIterations(20);
    // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
    // gtsam::GaussNewtonOptimizer optimizer ( graphWithPrior, *initial, params_gn );
    
    gtsam::Values result = optimizer_LM.optimize();//开始优化

3.提取优化结果

1. 顶点使用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();
        //fout<<"VERTEX_SE3:QUAT "<<key_value.key<<" "<<p.x() <<" "<<p.y() <<" "<<p.z() <<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" "<<endl;
    }

2. 边(因子)使用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 )
            {
                // write the edge information 
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();//LLT分解提取高斯模型信息矩阵
                gtsam::Pose3 pose = f->measured();//获得测量值
                gtsam::Point3 p = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();
                //fout<<"EDGE_SE3:QUAT "<<f->key1()<<" "<<f->key2()<<" "
                //    <<p.x() <<" "<<p.y() <<" "<<p.z() <<" "
                //    <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<" ";
                //由于gtsam定义的信息矩阵为斜对角线对称,转化为g2o定义下形式
                gtsam::Matrix infoG2o = gtsam::I_6x6;
                infoG2o.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
                infoG2o.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
                infoG2o.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
                infoG2o.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
                //for ( int i=0; i<6; i++ )
                //    for ( int j=i; j<6; j++ )
                //    {
                //        fout<<infoG2o(i,j)<<" ";
                //    }
                //fout<<endl;
            }
        }
    }