Gtsam优化库使用

Gtsam优化库使用：

参考博客

照着网上的例子以及一些开源代码中的格式加在自己的代码中，调了几个星期终于可以回环优化啦。
^ __ ^

我照着一些开源代码摸索了一下，用isam求解，记录一下，继续踩坑

1. 构建问题并优化

• 因子图模型（你的graph和initial value, 定义这些变量）
• 初始化 （初始化一些参数）
• 向initial中加入顶点（初始化顶点值）
• 添加边（对应到因子图graph中的因子）：构造因子，向图中添加因子
• 固定第一个顶点，在gtsam中相当于添加一个先验因子 PriorFactor到graph中

   //首先定义一堆要用的变量
   using namespace gtsam; //声明一下命名空间，后面就懒得加了
   
   // gtsam
   NonlinearFactorGraph gtSAMgraph; //因子图
   Values initialEstimate; //initial value 插入变量的初始值
   ISAM2 *isam; //后面用来优化的
   
   //isam initial parameters
   ISAM2Params parameters;
   parameters.relinearizeThreshold = 0.1;
   parameters.relinearizeSkip = 1;
   isam = new ISAM2(parameters);
   
   //然后根据代码中的一些结果，对应的顶点，边之类的加入到graph中
   //***** odometry *****
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6).finished()); 
   //对角矩阵，信息矩阵 这个的大小设置，以及各个元素之间的相对大小关系还在摸索中，反正先这样设了，后面再慢慢调整
   //========= PriorFactor =========
   gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(matrix_sum), priorNoise));
   //加入先验因子PriorFactor就是固定这个顶点
   initialEstimate.insert(0, trans2gtsamPose(matrix_sum));
   //insert vertex: currentID currentPose 将这个顶点的值加入initial value中
   //========= BetweenFactor ========
   gtsam::Pose3 poseFrom = trans2gtsamPose(keyframes[keyframeNum-1]->odom); //last pose
   gtsam::Pose3 poseTo   = trans2gtsamPose(matrix_sum);//current pose
   gtSAMgraph.add(BetweenFactor<Pose3>(keyframeNum-1, keyframeNum,  poseFrom.between(poseTo), odometryNoise));
   // poseFrom.between(poseTo) = poseFrom.inverse()*poseTo 在我这里是当前帧到上一帧的变换
   initialEstimate.insert(keyframeNum, poseTo);//insert vertex: currentID currentPose
   // update iSAM
   isam->update(gtSAMgraph, initialEstimate);
   isam->update();
   gtSAMgraph.resize(0);
   initialEstimate.clear();
   
   //***** loop closure *****
   Eigen::Matrix4f correctionLidarFrame = loop->relative_pose; 
   //pose between curr -> loop frame
   float noiseScore = loopdetector.getFitnessScore();
   Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
   noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
   gtSAMgraph.add(BetweenFactor<Pose3>(keyCurID, keyPreID, trans2gtsamPose(correctionLidarFrame), constraintNoise));
   //update isam
   isam->update(gtSAMgraph);
   isam->update();
   gtSAMgraph.resize(0);
   //新加入的要东西更新到isam中（也就是上面的isam->update...），
   //然后graph和initial value记得要清除掉，不然会重复更新的

//我习惯用eigen的矩阵，所以加了这个矩阵转换
gtsam::Pose3 trans2gtsamPose(Eigen::Matrix4f matrix_)
{
    gtsam::Rot3 rot3(matrix_(0,0), matrix_(0,1), matrix_(0,2),\
                     matrix_(1,0), matrix_(1,1), matrix_(1,2),\
                     matrix_(2,0), matrix_(2,1), matrix_(2,2));
    return gtsam::Pose3(rot3, gtsam::Point3(matrix_(0,3), matrix_(1,3), matrix_(2,3)));
}

Eigen::Matrix4f gtsam2transPose(gtsam::Pose3 pose)
{
    Eigen::Matrix4f matrix = Eigen::Matrix4f::Identity();
    matrix(0,3) = pose.translation().x();
    matrix(1,3) = pose.translation().y();
    matrix(2,3) = pose.translation().z();
    matrix.block<3,3>(0,0) = pose.rotation().matrix().cast<float>();
    return matrix;
}

2. 提取优化结果

   Values isamCurrentEstimate = isam->calculateEstimate();
   int isam_num = isamCurrentEstimate.size();
   for(int id = 0; id < isam_num; ++id)
   {
       //save key poses
       Pose3 latestEstimate = isamCurrentEstimate.at<Pose3>(id);
       Eigen::Matrix4f MM = gtsam2transPose(latestEstimate);
   }