gtsam使用教程

#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>

// 定义图变量
NonlinearFactorGraph gtSAMgraph;// 创建一个非线性的因子图
Values initialEstimate;// 初值估计
Values optimizedEstimate;// 优化后的值
ISAM2 *isam;
Values isamCurrentEstimate;

// 用于闭环图优化的参数设置，使用gtsam库
	ISAM2Params parameters;
		parameters.relinearizeThreshold = 0.01;
		parameters.relinearizeSkip = 1;
	isam = new ISAM2(parameters);//初始化了ISAM2对象

// 定义噪声矩阵，也就是协方差矩阵
noiseModel::Diagonal::shared_ptr priorNoise;// 先验噪声
noiseModel::Diagonal::shared_ptr odometryNoise;// 里程计噪声
noiseModel::Diagonal::shared_ptr constraintNoise; // 回环噪声
// 先验和里程计为固定值
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);
// 回环噪声为icp的匹配分数，分数越高越差
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);


// 第一步 添加先验因子
/**
* @brief 增加一个先验因子，目的是使图的位置固定
* @details PriorFactor  先验因子
* @param key 变量id
* @param Pose3 测量因子，也就是测量到的初始位姿，一般初始化为 0,0,0,0,0,0
* @param priorNoise 噪声矩阵
* @note Pose3 (const Rot3 &R, const Point3 &t) Construct from R,t. 从旋转和平移构造姿态 \n
* static Rot3 RzRyRx (double x, double y, double z),Rotations around Z, Y, then X axes \n
* 欧拉角的旋转RzRyRx依次按照z(transformTobeMapped[2])，y(transformTobeMapped[0])，x(transformTobeMapped[1])坐标轴旋转 \n
* xyz平移Point3 (double x, double y, double z)  Construct from x(transformTobeMapped[5]), y(transformTobeMapped[3]), and z(transformTobeMapped[4]) coordinates. 
*/ 
gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                                 Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));

/**
* @brief 为变量设置优化初值
* @details PriorFactor  先验因子
* @param key 变量id
* @param Pose3 优化初值
*/ 
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                      Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));


// 第二步 添加位姿变量和里程计因子并更新
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]));

// 构造函数原型:BetweenFactor (Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model)
// poseFrom.between(poseTo)两个位姿间的变换矩阵
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(gtSAMgraph, initialEstimate);
// update 函数为什么需要调用两次？
isam->update();

// 删除内容?
gtSAMgraph.resize(0);
initialEstimate.clear();

// Compute an estimate from the incomplete linear delta computed during the last update.
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);

thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity;
thisPose6D.roll  = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw   = latestEstimate.rotation().roll();
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);


// 第四步  闭环
gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);