gtsam使用总结（转）

原文链接

• 添加边(对应到因子图中的因子):构造因子,向图中添加因子
  构造因子所需参数:
  　连接顶点类型;
  　连接顶点的序号;
  　测量值;
  　高斯噪声模型,使用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 "<
      }
  

• 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 "<key1()<<" "<key2()<<" "
                  //    <
                  //    <
                  //由于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<
                  //    }
                  //fout<
              }
          }
      }
  

  ---

  补充：
  
  知乎gtsam学习笔记
  
  gtsam Github Examples

  
  # 寻找第三方库，使用大小写都可以，这里列举了两种方式
  
  find_package(Boost COMPONENTS thread filesystem date_time system REQUIRED)
  
  FIND_PACKAGE(GTSAM REQUIRED)
  
  
  # 包含第三方库头文件路径，可以使用绝对路径
  
  INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
  
  INCLUDE_DIRECTORIES(${GTSAM_INCLUDE_DIR})
  
  INCLUDE_DIRECTORIES("/usr/include/eigen3")
  
  
  add_executable(gtsam_test main.cpp)
  
  # 链接库
  
  target_link_libraries(gtsam_test ${Boost_LIBRARIES} -lgtsam -ltbb)
  
  install(TARGETS gtsam_test RUNTIME DESTINATION bin)
  
  

  clion没法识别的话就重启一下，可能是有的玩意没刷新出来。

  因子factor

  预定义的factor

  
  头文件
  
  #include  //二元因子，位姿之间，回环之间
  
  #include  //一元因子，系统先验
  
  //定义因子图
  
  NonlinearFactorGraph graph;
  
  //噪声定义 对角线矩阵：
  
  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
  
  //在因子图中加入一个因子
  
  // 二元因子
  
  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
  
  //参数解释 ^ ：因子类型 ^边 key 1、2 ^ 边的值 ^ 噪声模型
  
  // 一元因子
  
  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
  
  

  生成factor

  
  Rot2 prior = Rot2::fromAngle(30 * degree);
  
  prior.print("goal angle");
  
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  
  Symbol key('x',1); // 一个key就是一个label
  
  PriorFactor<Rot2> factor(key, prior, model);
  
  

  初值定义

  一个图中要给每一个变量赋予一个初始值。

  
  Values initialEstimate; //定义初始值容器
  
  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); //加入一个变量 arg1：变量的标签 arg2:这个变量的值
  
  

  噪声定义

  gtsam中的噪声有很多类型

  namespace NM = gtsam::noiseModel;

  
  // Set Noise parameters
  
  Vector priorSigmas = Vector3(1,1,M_PI);
  
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  
  const NM::Base::shared_ptr // 基类型
  
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior 对角线噪声
  
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust 各项同性噪声
  
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  
  

  优化方法

  GaussNewton法

  引入 #include

  
  GaussNewtonParams parameters;//参数对象
  
  parameters.relativeErrorTol = 1e-5; // 迭代变化量小于这个值就退出
  
  parameters.maxIterations = 100; //最大迭代次数
  
  //创建一个高斯牛顿优化器 arg： 因子图 初值 参数对象
  
  GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
  
  //求出优化解 得到变量的最优值
  
  Values result = optimizer.optimize();
  
  

  LevenbergMarquardt法

  引入 #include

  
  LevenbergMarquardtOptimizer optimizer(graph,initialEstimate);
  
  Values result=optimizer.optimize();
  
  

  边缘化 marginal

  
  Marginals marginals(graph,result);
  
  for (int j = 1; j < 4; ++j) {
  
  boost::format fmt("\n x%1% covariance: \n %2% \n");
  
  fmt%j%marginals.marginalCovariance(j); //得到每个变量的协方差
  
  cout<<fmt;
  
  }
  
  

  读写 g2o文件

  
  #include  //引入头文件
  
  //读
  
  NonlinearFactorGraph::shared_ptr graph1;
  
  Values::shared_ptr initialEstamate1;
  
  string g2ofile=findExampleDataFile("noisyToyGraph.txt");
  
  bool is3D =false;
  
  boost::tie(graph1,initialEstamate1)=readG2o(g2ofile,is3D);
  
  //写 g2o文件
  
  writeG2o(graph,result,"graph2g2o.g2o"); //args: 因子图 ，变量结果，目标文件名
  
  

  iSAM 更新过程

  在文件ISAM2.cpp的update函数中

  1. 添加新的因子

  Impl::AddFactorsStep1

  
  //关键变量
  
  ISAM2Result result;
  
  isam.update(graph, initialEstimate);
  
  ISAM2Params params_; //在定义ISAM2实例的时候存储参数的。
  
  // parameters.relinearizeThreshold = 0.01;
  
  // parameters.relinearizeSkip = 1;
  
  Impl //Internal implementation functions 内部实施函数
  
  useUnusedSlots //slot 插槽
  
  

  2. 初始化新的变量 Impl::AddVariables

  $$\Theta :=\Theta \cup {\Theta }_{new}.$$

  3. 标记线性化更新

  4. 标记 变化量大于阈值的key

  $$\beta :J=\{{\Delta }_j\in \Delta |{\Delta }_j\ge \beta \}.$$

  5. 标记所有与变量相关小团体clique 以及他们的所有父节点

  6. 更新线性化点对于标记的变量

  $${\Theta }_J:={\Theta }_J+{\Delta }_J.$$

  7. 线性化 新的因子

  8. 重新整理贝叶斯数的树顶

  LevenbergMarquardtOptimizer

  LevenbergMarquardtOptimizer继承自 NonlinearOptimizer的optimize->defaultOptimize iterate();(这个函数是在子类中实现的，不是在父类中实现的)

  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  virtual const Values& optimize() { defaultOptimize(); return values(); }
  

  在iterate中：
  KaTeX parse error: Undefined control sequence: \f at position 2: \̲f̲ ̲Ax-b \approx h(…

  1、
  GaussianFactorGraph::shared_ptr linear = linearize();
  就是对每一个因子进行线性化：NonlinearFactor：：linearize（）
  2、
  whitenedError 白化后的噪声