系列文章链接：
【SLAM】LIO-SAM解析——数据预处理imageProjection(2)
【SLAM】LIO-SAM解析——特征提取featureTrack(3)
【SLAM】LIO-SAM解析——IMU预计分IMU-Preintegration(4)
【SLAM】LIO-SAM解析——后端优化MapOptimization(5)
【SLAM】LIO-SAM解析——里程计融合transformFusion(6)
知识点：
如何使用GTSAM，从而输出一个丝滑的位姿；
已知上一帧lidar里程计(频率低)，当前帧lidar里程计(频率低)，两个lidar帧之间的IMU数据，如何利用图优化的方式优化位姿状态量(PVQ)和IMU状态量(noise，bias)；
如何结合lidar里程计(频率低)和IMU数据(频率低)输出IMU里程计(频率高)。
如何使用两个预积分器，一个服务于图优化，一个服务于imu里程计的计算。
4. 综述
从看代码的角度讲，LIO-SAM涉及到的数学好像不是很多，基本GTSAM都给你搞定了，所以知道相关的基本原理就能看懂代码。这部分内容对应IMUPreintegration类，这个类对外的作用是根据低频率的lidar里程计和高频率的IMU数据，输出一个高频率的IMU里程计。为了保证输出的里程计的精度，作者采用GTSAM+图优化的方式来做这个事情。
LIO-SAM涉及到4个.cpp文件，有5个主要的类，同时有多个发布和订阅的数据，必须要搞清楚当前这个类这个函数订阅的数据是哪来的，频率是什么样的，发布的是什么数据，会被谁订阅，详情请见这个流程图。
我自己的思考：把低频的lidar里程计转化为高频的IMU里程计，直接简单的在当前帧上进行IMU积分可不可以，一定要用算力昂贵的GTSAM吗，一定要用复杂的传来传去的数据传输吗，一定要进行图优化吗？当业务的参考模型是LIO-SAM的时候，当我的机器人硬件资源有限的时候，我一定要按照作者相同的玩法来吗？
在这个博客讲解的过程中，是按照过程的顺序或者说代码的顺序来的，同时避免了大段大段的复制代码，反正我看别人的博客一上来大段大段代码粘上去，我是新手的时候还是挺难看进去的。
这个类包含两个主要的函数：
odometryHandler():
获取低频lidarpose，配上缓存的IMU数据，进行图优化，维护预积分器。
输入：来自mapOptimization的lidar里程计。
输出：无。
imuHandler():
利用预积分器，根据实时IMU数据，输出IMU里程计。
输入：IMU数据；
输出：lidar系下的imu里程计，会被imageProjection和tranformFusion订阅。
4.1 IMUPreintegration入口
同样看到它的构造函数，
IMUPreintegration()
{
// 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的imu预计分量，预测每一时刻（imu频率）的imu里程计
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 发布imu里程计
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);2个订阅1个发布，
2个订阅：
高频率的IMU数据；
低频率的lidar里程计；
1个发布：
高频率的IMU里程计，随imuHandler()发布。
然后设置IMU预积分的噪声方差：
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias分别是重力加速度大小，线加速度噪声协方差，角速度噪声协方差，积分速度到位移的协方差和bias，出来后两个是拍脑袋写的，头三个都是在utility.h从配置文件中读取的。
接下来设置的是状态量：位移，速度和bias的先验噪声，
priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good然后是两帧之间pose的噪声和bias的噪声：
correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();如果输入的lidar里程计有较大的误差，那么pose噪声就使用correctionNoise2，一般情况用correctionNoise。
最后是2个预积分器，分别用于图优化中使用和输出IMU里程计时使用：
// imu预积分器，用于预测每一时刻（imu频率）的imu里程计（转到lidar系了，与激光里程计同一个系）
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
// imu预积分器，用于因子图优化
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization 4.2 输入数据为lidar里程计时的回调函数odometryHandler()
这个函数包含了IMUPreintegration最核心的内容，就是如何利用输入的lidar pose和IMU数据进行图优化，获得一个最合理的状态量估计值，包括PVQ，各种noise，bias。
4.2.1 输入数据的预处理
当拿到一帧最新的lidar pose(频率低)时，要获取它的时间，还得保证IMU buffer里有东西，都OK的话就把lidar pose拿出来。里面有一个odomMsg->pose.covariance[0]，这个东西不是方差，放了一个标志位，告诉你这个lidar pose准不准，1是不准，0是准，分别对应了不同的方差。
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// 当前帧激光里程计时间戳
double currentCorrectionTime = ROS_TIME(odomMsg);
// 确保imu优化队列中有imu数据进行预积分
if (imuQueOpt.empty())
return;
// 当前帧激光位姿，来自scan-to-map匹配、因子图优化后的位姿
float p_x = odomMsg->pose.pose.position.x;
float p_y = odomMsg->pose.pose.position.y;
float p_z = odomMsg->pose.pose.position.z;
float r_x = odomMsg->pose.pose.orientation.x;
float r_y = odomMsg->pose.pose.orientation.y;
float r_z = odomMsg->pose.pose.orientation.z;
float r_w = odomMsg->pose.pose.orientation.w;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));4.2.2 输入的lidar pose是第一帧时的初始化：添加先验信息
首先是把优化器相关内容重置一下，
if (systemInitialized == false)
{
// 重置ISAM2优化器
resetOptimization();我们进去看看是怎么重置的：
void resetOptimization()
{
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1;
optParameters.relinearizeSkip = 1;
optimizer = gtsam::ISAM2(optParameters);
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}可以发现他把优化器optimizer，因子graphFactors和属性值graphValues干脆都换成新的了。注意了，在GTSAM图优化里，optimizer，graphFactors和graphValues是三个重要的工具对象。
然后从imu优化队列中删除当前帧激光里程计时刻之前的imu数据：
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}接下来开始正菜了，往因子图里添加里程计位姿先验因子：
prevPose_ = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);lidarpose是lidar->world的变换，首先当然得转化为IMU->world的形式，prevPose_是因子priorPose的数值，然后把这个因子加到因子图graphFactors里。
类似的，添加速度先验因子：
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);卸磨杀驴：
graphFactors.resize(0);
graphValues.clear();把优化后的prevBias_给到预积分器：
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);最后告诉自己，初始化完成，key是循环的计数。初始化后直接return掉，进入下次循环。
key = 1;
systemInitialized = true;
return;
}4.2.3 每隔100帧激光里程计，重置优化器
随着时间的运行，图的规模越来越大，内存和时间会越来越大，所以每100帧就把优化器重置一次。过程和初始化很类似，只不过状态量是前一帧的数据，噪声协方差是边缘化之后留下来的，要是作者写成滑动窗口就好了。
if (key == 100)
{
// 前一帧的位姿、速度、偏置噪声模型
gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
// 重置ISAM2优化器
resetOptimization();
// 添加位姿先验因子，用前一帧的值初始化
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// 添加速度先验因子，用前一帧的值初始化
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// 添加偏置先验因子，用前一帧的值初始化
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
graphFactors.add(priorBias);
// 变量节点赋初值，用前一帧的值初始化
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// 优化一次
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}4.2.4 正常情况下的图优化
首先把IMU数据导入到用于优化的IMU预积分器里：
while (!imuQueOpt.empty())
{
// 提取前一帧与当前帧之间的imu数据，计算预积分
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
if (imuTime < currentCorrectionTime - delta_t)
{
// 两帧imu数据时间间隔
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
// imu预积分数据输入：加速度、角速度、dt
imuIntegratorOpt_->integrateMeasurement(
gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuT_opt = imuTime;
// 从队列中删除已经处理的imu数据
imuQueOpt.pop_front();
}
else
break;
}添加imu预积分因子：
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
// 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预计分量
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);添加imu bias因子，前一帧bias，当前帧bias，观测值，噪声协方差；deltaTij()是积分段的时间：
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));添加位姿因子：看到了吗，根据位姿准不准，它添加的协方差是不一样的，
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);现在呢，从上一帧到当前帧之间的IMU有了，上一帧状态量也有，当前帧输入的里程计也有，那么就可以获取图优化之后当前帧的一个初始预测值了：
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);以上内容是完成graphFactors的配置，接下来配置graphValues：
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);由于key-1帧的graphValues已经有了，所以只用配置key帧的数据。
优化：
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();拿到结果：
gtsam::Values result = optimizer.calculateEstimate();把当前帧得到的结果给到prev帧，留给下个循环用：
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
// 更新当前帧状态
prevState_ = gtsam::NavState(prevPose_, prevVel_);
// 更新当前帧imu偏置
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
// 重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
imu因子图优化结果，速度或者偏置过大，认为失败：
if (failureDetection(prevVel_, prevBias_))
{
// 重置参数
resetParams();
return;
}4.2.5 构建用于imu里程计的预积分器
上一步优化的结果给到用于输出imu里程计的预积分器里：
首先更新最新一帧lidarpose的位姿，速度，bias：
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;从imu队列中删除当前激光里程计时刻之前的imu数据：
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}这里要特别注意一下，这个IMU的buffer和之前的不一样，imuQueImu是用于发布IMU里程计的buffer，而之前的imuQueOpt是用于优化的buffer。
如果现在IMU buffer还有数据，那么就加到用于发布IMU里程计的IMU预积分器里，会在imuHandler()中被用到：
if (!imuQueImu.empty())
{
// 重置预积分器和最新的偏置
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// 计算预积分
for (int i = 0; i < (int)imuQueImu.size(); ++i)
{
sensor_msgs::Imu *thisImu = &imuQueImu[i];
double imuTime = ROS_TIME(thisImu);
double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuQT = imuTime;
}
}更新帧号key，同时告诉imuHandler()我可以的：
++key;
doneFirstOpt = true;
}4.3 输入数据为IMU数据时的回调函数imuHandler()
这个函数是发布IMU里程计的，由于预积分器在odometry()中被更新了，相关的信息都在里面，那么就可以根据输入的高频IMU数据发布同样高频的IMU里程计信息。
当来了IMU数据的时候，首先对IMU数据进行坐标转换，转化到lidar系下：
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
// imu原始测量数据转换到lidar系，加速度、角速度、RPY
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);注意了，由于IMU数据不包括位移，所以这个坐标变换只涉及到旋转。在odometryHandler()里，我们可以看到gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu)，他说把lidarpose转换到IMU系，但是这里又是把IMU数据转到lidar系，岂不是冲突了吗？我们再看看lidar2Imu：
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));只涉及到平移的变换，不涉及旋转变换。所以说，整个类，是在lidar坐标系下进行的，原点却是body坐标系的原点。
然后添加当前帧imu数据到队列：
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分重新计算了：
if (doneFirstOpt == false)
return;把来的最新一个imu数据加到预积分器里：
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// imu预积分器添加一帧imu数据，注：这个预积分器的起始时刻是上一帧激光里程计时刻
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);用当前已经收到的最新的激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态：
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);在odometryHandler()的最后面，这个预积分器已经加入了一些lidarpose时刻后面收到的，但是在当前imu时刻之前的一些imu数据，但是因为imuHandler()和odometryHandler()都上锁了，所以不用担心把担心这个imu数据重复的加入到预积分器里。
最后，把当前预测的imu里程计包装成message发布出去就大功告成了，注意这次是在lidar坐标系下，不光是旋转，平移也是lidar系下的：
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// 变换到lidar系
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
odometry.pose.pose.position.x = lidarPose.translation().x();
odometry.pose.pose.position.y = lidarPose.translation().y();
odometry.pose.pose.position.z = lidarPose.translation().z();
odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
odometry.twist.twist.linear.x = currentState.velocity().x();
odometry.twist.twist.linear.y = currentState.velocity().y();
odometry.twist.twist.linear.z = currentState.velocity().z();
odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
pubImuOdometry.publish(odometry);
}
880
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
