SLAM中的常用优化器-----GTSAM篇 • Jianghc's Blog

最近在读LIO-SAM代码中看到了很多有关于GTSAM优化器的使用，由于之前一直用的是ceres，因此把代码中有关GTSAM的相关用法记录下，不知道能否形成一个模板。

常见数据数据结构和状态#

gtsam::noiseModel::Diagonal::shared_ptr 定义噪声模型下的协方差矩阵
gtsam::PreintegratedImuMeasurements
IMU预积分器相关类
gtsam::Pose3 一个位姿的基本单位，继承自李群, 满足常规的相关运算。包含旋转(Rot3 Rotation)和平移(Point3 translation)两个主要的属性
相关类的官方文档对的定义如下：
Pose3 ↗
gtsam::Vector3 这个本身不是gtsam中自带的数据类型，是Vector.h文件中重新进行tpyedef的变量名字，是利用的eigen库进行的重命名，typedef Eigen::Vector3d Vector3;

gtsam::NavState 导航状态变量(应用于IMU因子中)，包含Pose (rotation, translation) + velocity NOTE(frank)，已经不在李群上，是一个9D的流形空间 NavState ↗
gtsam::imuBias::ConstantBias IMU的两个bias计算和更新的类，包含biasAcc和biasGyro，里面自带函数通过两个bias完成正确的加速度和角速度的更新。correctAccelerometer和correctGyroscope ConstantBias ↗
gtsam::PreintegratedImuMeasurements->integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt ) 添加imu的测量数据, 用于计算IMU的预积分结果，每有一帧的IMU信息加入即可添加，包括测量的加速度和角速度以及相邻帧的时间差
gtsam::PreintegratedImuMeasurements->predict(const NavState &state_i, const imuBias::ConstantBias &bias_i) 有返回返回值gtsam::NavState，完成IMU状态量的预测，输入时上一阵的IMU状态以及上一阵的偏置量，根据输入的当前测量，完成状态量的预测
gtsam::noiseModel::Gaussian::Covariance(const Matrix& covariance, bool smart=true) 通过协方差矩阵构建高斯噪声模型，用于添加因子时进行输入 Gaussian::Covariance ↗

一个通用的模板操作#

添加先验因子----一元边

// 添加位姿先验因子
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);

// 添加速度先验因子
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);

// 添加imu偏置先验因子
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);

// 优化节点添加优化初值
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);

// 优化一次
optimizer.update(graphFactors, graphValues);

// 将因子和变量清0
graphFactors.resize(0);
graphValues.clear();

// 重置预积分优化和bias
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

plaintext

gtsam在安装过程中出现的问题#

前情提要 gtsam在安装的时候使用自带的eigen，这样使得通常和电脑中原先装有的eigen版本冲突，因此需要更改gtsam在编译过程中使用的默认的eigen优化器， 解决方法 在CMakeLists.txt文件中if(GTSAM_USE_SYSTEM_EIGEN)之前加上set(GTSAM_USE_SYSTEM_EIGEN ON), 再重新编译安装！

另外还有一点，对gtsam进行cmake的时候要写为：

cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..

plaintext

否则launch的时候会出错。

python部分的GTSAM使用总结#

噪声模型当中的cov的值代表观测的不确定性程度，cov的值越小则对应该观测越鲁棒和稳定，因此不确定性越低，而cov的值越大则表示该观测越不鲁棒和稳定，因此在该优化当中的椭球可能更大，不确定性更大，则更多的被优化？