0. 前言

在深入剖析了CeresEigenSophusG2O后,以V-SLAM为代表的计算方式基本已经全部讲完,但是就L-SLAM而言我们还差一块,那就是PCL、GTSAM点云计算部分我们还没有详细的去写,正好就这个时间,我想把这块坑给填完,来形成一整个系列,方便自己回顾以及后面的人一起学习。

GTSAM系统认知

我们在一般的认知,G2O和GTSAM都做着后端图优化的功能,而他们当中的全局优化器的性能却是不尽相同。

  • Ceres是针对所有种类的函数的优化,所以只能用最速梯度下降法。
  • G2o是求解的一个标准的最小二乘
    在这里插入图片描述
  • GTSAM准确的说不是解的优化方程。而是先把一个概率图的联合概率的信息矩阵和信息向量表示出来
    在这里插入图片描述
    但具有物理意义的是均值和协方差。所以通过信息矩阵和协方差矩阵的换算计算出均值。这个换算里面有求逆,等于是在解方程。
    如图所示,我们在下图轨迹中进行evo误差估计,发现G2O效果其实略好于GTSAM,但是为了我们系统的完整性,我们仍然需要讲一下GTSAM。
    在这里插入图片描述

在这里插入图片描述

2. GTSAM/ISAM2示例

iSAM2 将图优化问题转换成 Bayes tree 的建立、更新和推理问题。是GTSAM函数库中常用的一种方法(LeGO-LOAM中使用的即为ISAM2)。下面我们针对GTSAM和ISAM2来从两个层面理清楚写法上的差异。这部分的代码参考了这篇文章
在这里插入图片描述
GTSAM

#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) 
{
    NonlinearFactorGraph graph;

    Values initials;
    initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
    initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
    initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
    initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
    initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));
    initials.print("\nInitial Values:\n"); 
    //固定第一个顶点,在gtsam中相当于添加一个先验因子
    noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 0.1));
    graph.add(PriorFactor<Pose2>(Symbol('x', 1), Pose2(0, 0, 0), priorModel));

    //二元位姿因子
    noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));

    //二元回环因子
    noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));
    graph.print("\nFactor Graph:\n"); 


    GaussNewtonParams parameters;
    parameters.setVerbosity("ERROR");
    parameters.setMaxIterations(20);
    parameters.setLinearSolverType("MULTIFRONTAL_QR");
    GaussNewtonOptimizer optimizer(graph, initials, parameters);
    Values results = optimizer.optimize();
    results.print("Final Result:\n");

    Marginals marginals(graph, results);
    cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
    cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
    cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
    cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
    cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;

    return 0;
}

ISAM

#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>

using namespace std;
using namespace gtsam;

int main()
{

  // 向量保存好模拟的位姿和测量,到时候一个个往isam2里填加
  std::vector< BetweenFactor<Pose2> > gra;
  std::vector< Pose2 > initPose;

  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

  gra.push_back(BetweenFactor<Pose2>(1, 2, Pose2(5, 0, 0     ), model));
  gra.push_back(BetweenFactor<Pose2>(2, 3, Pose2(5, 0, -M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(3, 4, Pose2(5, 0, -M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(4, 5, Pose2(5, 0, -M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(5, 2, Pose2(5, 0, -M_PI_2), model));

  initPose.push_back(Pose2(0.5, -0.0,  0.2   ));
  initPose.push_back( Pose2(5.3, 0.1, -0.2   ));
  initPose.push_back( Pose2(10.1, -0.1,  -M_PI_2));
  initPose.push_back( Pose2(10.0, -5.0,  -M_PI  ));
  initPose.push_back( Pose2(5.1, -5.1, M_PI_2));

  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.01;
  parameters.relinearizeSkip = 1;
  ISAM2 isam(parameters);

  // 注意isam2的graph里只添加isam2更新状态以后新测量到的约束
  NonlinearFactorGraph graph;
  Values initialEstimate;

  // 第一个姿势不需要更新
  for( int i =0; i<5 ;i++)
  {
          // 添加对当前姿势的初步猜测
           initialEstimate.insert(i+1, initPose[i]);

           if(i == 0)
           {
               //  在第一个姿势上添加一个前置,将其设置为原点
               //一个先验因子由一个均值和一个噪声模型组成(协方差矩阵)
               noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
               graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));

           }else
           {

               graph.push_back(gra[i-1]);  // Ie:当I = 1时,机器人在pos2,在pos1和pos2之间有一条边gra[0]
               if(i == 4)
               {
                   graph.push_back(gra[4]);  //  当机器人在pos5时,有两条边,一条是pos4 ->pos5,另一条是pos5->pos2 (grad[4])
               }
               isam.update(graph, initialEstimate);
               isam.update();

               Values currentEstimate = isam.calculateEstimate();
               cout << "****************************************************" << endl;
               cout << "Frame " << i << ": " << endl;
               currentEstimate.print("Current estimate: ");

               // 特别重要,update以后,清空原来的约束。已经加入到isam2的那些会用bayes tree保管,你不用操心了。
               graph.resize(0);
               initialEstimate.clear();
           }
  }
  return 0; 
}

在这里插入图片描述
如果对于三维位姿,我们只需要改变第二步即可。因为这部分的计算与二维位姿不同之处就在于,三维位姿要使用pose3,由欧拉角和t组成。

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) 
{
    NonlinearFactorGraph graph;

    Values initials;
    initials.insert(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
    initials.insert(Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(6, 0.1, -1.1)));
    initials.insert(Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10.1, -0.1, 0.9)));
    initials.insert(Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10, 5, 0)));
    initials.insert(Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5.1, 4.9, -0.9)));
    initials.print("\nInitial Values:\n"); 
    //固定第一个顶点,在gtsam中相当于添加一个先验因子
    noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
          (Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); 
    graph.add(PriorFactor<Pose3>(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));

    //二元位姿因子
    noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
    graph.add(BetweenFactor<Pose3>(Symbol('x', 1), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(Symbol('x', 2), Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(Symbol('x', 3), Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(Symbol('x', 4), Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), odometryNoise));

    //二元回环因子
    noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Variances(
          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
    graph.add(BetweenFactor<Pose3>(Symbol('x', 5), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, -5, 0)), loopModel));
    graph.print("\nFactor Graph:\n"); 


    GaussNewtonParams parameters;
    parameters.setVerbosity("ERROR");
    parameters.setMaxIterations(20);
    parameters.setLinearSolverType("MULTIFRONTAL_QR");
    GaussNewtonOptimizer optimizer(graph, initials, parameters);
    Values results = optimizer.optimize();
    results.print("Final Result:\n");

    Marginals marginals(graph, results);
    cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
    cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
    cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
    cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
    cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;

    return 0;
}

在这里插入图片描述

Lego-LOAM 因子图优化

研究了一下LeGo-LOAM中的因子图优化逻辑:

//系列头文件
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
//系列声明与初始化
using namespace gtsam;
NonlinearFactorGraph gtSAMgraph;
Values initialEstimate;
Values optimizedEstimate;
ISAM2 *isam;
Values isamCurrentEstimate;
noiseModel::Diagonal::shared_ptr priorNoise;
noiseModel::Diagonal::shared_ptr odometryNoise;
noiseModel::Diagonal::shared_ptr constraintNoise;
//初始化
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
gtsam::Vector Vector6(6);
Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
priorNoise = noiseModel::Diagonal::Variances(Vector6);
odometryNoise = noiseModel::Diagonal::Variances(Vector6);
.......
//在回环检测线程中,当找到闭环,就添加回环因子
 gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
 gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
 gtsam::Vector Vector6(6);
 float noiseScore = icp.getFitnessScore();
 Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
 constraintNoise = noiseModel::Diagonal::Variances(Vector6);
 /* add constraints*/
 std::lock_guard<std::mutex> lock(mtx);
 gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
 isam->update(gtSAMgraph);
 isam->update();
 gtSAMgraph.resize(0);
 .......
 //在保存关键帧和因子中,先是添加固定先验因子
 if (cloudKeyPoses3D->points.empty())
{
     gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
     initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
     for (int i = 0; i < 6; ++i)
         transformLast[i] = transformTobeMapped[i];
}
.......
//随后添加位姿间的二元因子
else
{
    gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),Point3(transformLast[5], transformLast[3], transformLast[4]));
    gtsam::Pose3 poseTo   = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
    gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
    initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
}
//更新isam和计算位姿
/*** update iSAM*/
isam->update(gtSAMgraph, initialEstimate);
isam->update();

gtSAMgraph.resize(0);
initialEstimate.clear();
/** save key poses*/
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;

isamCurrentEstimate = isam->calculateEstimate();//计算出最终矫正后结果存放的地方
//访问最终矫正后的位姿
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
    cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().y();
    cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().z();
    cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().x();

    cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
    cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
    cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
    cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
    cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
    cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
}

4. 参考链接

https://www.guyuehome.com/24812
https://blog.csdn.net/fuxingyin/article/details/51854415?utm_medium=distribute.pc_relevant.none-task-blog-title-1&spm=1001.2101.3001.4242
https://www.cnblogs.com/reinforce/p/5510539.html