【3D激光SLAM】 LIO_SAM学习——IMU预积分imuPreintegration
前言
LIO_SAM一共有四个cpp文件，接下来对其中一个imuPreintegration.cpp文件，根据数据流程进行详细的讲解，不足的地方请大家批评指证
该文件主要包括：一个主函数，两个回调函数（odometryHandler() 和 imuHandler() ）
一、主函数main()
主要是创建两个对象，调用构造函数
int main(int argc, char** argv)
{
//节点初始化
ros::init(argc, argv, "roboat_loam");
//创建 IMUPreintegration 的一个对象，会调用构造函数IMUPreintegration （）
IMUPreintegration ImuP;
//创建 TransformFusion 的一个对象，会调用构造函数TransformFusion （）
TransformFusion TF;
//打印
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
//开四个线程 通过并发的方式使得速度得到提升
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
二、构造函数IMUPreintegration （）
IMUPreintegration （）构造函数，主要包含：两个订阅与一个发布器
两个订阅的回调函数为:
imuHandler() 订阅高频率的IMU数据；
odometryHandler（）订阅 低频率的lidar里程计；
发布器：pubImuOdometry 发布器 ，发布高频率的imu里程计，在imuHandler()中执行
// @brief 构造函数包含两个订阅与与一个发布
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);
// 设置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
// 先验位姿噪声 包括姿态以及位置
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
// 激光里程计scan-to-map优化过程中若发生退化 则选择一个较大的方差
// 如果输入的lidar里程计有较大的误差，那么pose噪声就使用correctionNoise2，一般情况用correctionNoise
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();
//两个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
}
三、imuHandler()
订阅imu的原始数据，imu原始数据到来之后，先用imuConverter()转换到雷达系下，但其实和雷达之间仍然差了一个平移，然后将转换后的imu数据，分别放到两个双端队列中（imuQueOpt与 imuQueImu），简单介绍一下这两个队列，后面会详细解释，imuQueOpt用来预测每个imu时刻的imu里程计，而imuQueImu用于因子图优化。初始的时候doneFirstOpt 为false，函数return
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数据到队列
// 两个双端队列分别装着优化前后的imu数据
imuQueOpt.push_back(thisImu); //用于预测每个时刻（IMU频率）imu里程计 用来执行预积分和位姿变换的优化
imuQueImu.push_back(thisImu); //用于因子图优化 用来更新最新imu状态
// 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分已经被重新计算
// 这里需要先在odomhandler中优化一次后再进行该函数后续的工作
if (doneFirstOpt == false)
return;
后面内容咱暂时省略。。。。。。。。。。。。。。。
四、odometryHandler（）
很多篇博客都说，这个函数比较抽象，难理解，接下一起走进这个函数，逐行讲解
4.1 输入的雷达里程计预处理
订阅雷达里程计数据，获取当前激光帧里程计的时间戳，odomMsg来自scan-to-map匹配、因子图优化后的位姿，然后用一标志位，告诉你这个lidar pose准不准，1是不准，0是准，分别对应了不同的方差， 然后将位姿转换为gtsam格式
// 订阅的是激光里程计,"lio_sam/mapping/odometry_incremental" 由mapoptimization发布的
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// 当前激光帧里程计时间戳 此处的odomMsg数据 是相对与odom坐标系的绝对数据
double currentCorrectionTime = ROS_TIME(odomMsg);
// make sure we have imu data to integrate
// 确保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;
// 判断扫描匹配的过程中是否发生退化 为1 则 雷达里程计有退化风险，该帧位姿精度有一定程序下降
// odomMsg->pose.covariance[0]，这个东西不是方差，放了一个标志位，
// 告诉你这个lidar pose准不准，1是不准，0是准，分别对应了不同的方差
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
//imu预积分用到gtsam
// 定义gtsam格式 的当前雷达的位姿（此处应该是基于map坐标系的位姿）
// 将位姿转成 gtsam的格式
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 系统初始化------输入第一帧激光里程计数据
第一帧激光里程计进来时，systemInitialized 初始时为false， resetOptimization()重置isam2优化器
//initialize system
// 系统初始化 第一帧
if (systemInitialized == false)
{
// 重置isam2优化器
resetOptimization();
紧着下文
分析一下resetOptimization()
在GTSAM图优化里，优化器optimizer，因子图graphFactors和状态信息graphValues是三个重要的工具对象。
// 重置优化器 优化器 有什么作用实在想不懂 imu预积分还要什么优化器
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;
}
书接上文，重置isam2优化器后，从mu队列imuQueOpt中删除当前帧激光里程计时刻之前的imu数据， imuQueOpt存储的imu信息用于预积分，lastImuT_opt 记录着，早于当前帧激光里程计时间戳，且离其最近的一帧imu数据时间戳
// 从imu队列中删除当前帧激光里程计时刻之前的imu数据 delta_t=0
while (!imuQueOpt.empty())
{
// 从imuQueOpt队列中 删除时间戳在 当前激光里程计时间戳之前的imu数据 delta_t=0
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
//记录当前激光里程计数据中 时间上早于其且离其最近的一帧IMU数据
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
//弹出IMU数据
imuQueOpt.pop_front();
}
else
break;
}
初始化pose，往因子图添加里程计位姿先验因子
补充一下：imuConverter()并没有涉及到平移，lidar2Imu or imu2Lidar 却只有平移的内容
//initial pose
//lidarPose 为本回调函数收到的激光里程计数据，重组成gtsam格式的pose
//并转到imu坐标系下,猜测compose可能类似于左乘之类的含义吧
//得到激光帧先验位姿(lidarPose)转换到imu系下,（相当于从真正的雷达系扭到上面的中间系中）
prevPose_ = lidarPose.compose(lidar2Imu);
//X可能是固定搭配（当使用Pose时），如果是速度则是V，bias则是B
// 此处定义一个模型 第一个参数：X(0)表示模型的名字 第二个参数：模型的均值 第三个参数：模型的噪声
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
//通过调用总的因子图模型的add方式，添加第一个因子
//PriorFactor 概念可看gtsam 包括了位姿 速度 bias
//加入PriorFactor在图优化中基本都是必需的前提
//各种noise都定义在构造函数当中
// 往因子图里添加里程计位姿先验因子：
graphFactors.add(priorPose); //理论模型计算值 prevPose_是因子priorPose的数值
初始化速度，添加速度先验因子
// initial velocity
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel); //添加速度先验因子
初始化imu偏置，添加imu偏置先验因子
// initial bias
prevBias_ = gtsam::imuBias::ConstantBias(); //imuBias因子 后面使用时可能会用到
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias); //添加imu偏置先验因子
添加状态信息
// add values
// 变量节点赋初值
// 实际测量值
// 添加状态信息
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
进行优化，输入graphFactors因子图与graphValues状态信息
// optimize once
// 优化一次
// 思考此处的优化 与mapoptimaztion中的优化有何区别(暂时研究不出来区别) 以及其为什么需要优化啊？ 预积分还需要优化？ 好迷啊
// 给各个状态量赋成初始值
// graphFactors因子图， graphValues状态信息
optimizer.update(graphFactors, graphValues);
因子图与状态信息清空
//图和节点均清零
//节点信息保存在gtsam::ISAM2 optimizer，所以要清理后才能继续使用
graphFactors.resize(0);
graphValues.clear();
//积分器重置,重置优化之后的偏置
// 预积分的接口，使用初始零偏进行初始化 之前imu有两个队列，每个队列对应预积分处理器
// 把优化后的prevBias_给到预积分器
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
初始化完成，key 用于计数，然后return，多说一句，key累加到100，一个循环
key = 1;
systemInitialized = true;
return;
}
imu的频率较快，比激光雷达快 ，存在imuTime < currentCorrectionTime，
计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，
添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
IMU数据（加速度、角速度、dt）导入到用于优化的IMU预积分器里
while (!imuQueOpt.empty())
{
// 提取前一帧与当前帧之间的imu数据，计算预积分
//获取imu第一帧数据的时间戳
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
//转成ROS时间
double imuTime = ROS_TIME(thisImu);
// cwx
// 因为初始化后直接return
// 由于imu频率比较快，比激光雷达快 ，存在imuTime < currentCorrectionTime
// currentCorrectionTime 是当前回调函数收到的激光里程计数据的时间
// 初始化时才会剔除, 当前帧之前的imu数据并直接退出，其他非初始化情况时，早于当前currentCorrectionTime的imu数据反倒不会被剔除而是用于积分计算
if (imuTime < currentCorrectionTime - delta_t)
{
// lastImuT_opt在初始化时，被赋值为离激光雷达当前帧最近的imu数据时间戳，imu时间戳小于激光雷达时间戳,
// imuTime - lastImuT_opt 计算的是两帧连续imu数据的时间差值
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
// imu预积分数据输入：加速度、角速度、dt
// imuIntegratorOpt_用来因子图优化的预积分器, 注意加入了上一步算出的dt
//作者要求的9轴imu数据中欧拉角在本程序文件中没有任何用到,全在地图优化里用到的
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因子到因子图
//利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中,
//注意后面容易被遮挡，imuIntegratorOpt_的值经过格式转换被传入preint_imu，
//因此可以推测imuIntegratorOpt_中的integrateMeasurement函数应该就是一个简单的积分轮子（可以直接套用），
//传入数据和dt，得到一个积分量,数据会被存放在imuIntegratorOpt_中
// dynamic_cast<将要转换成的新类型> data 显式类型转换
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
// 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预积分量 注意此帧指的是（激光雷达帧）
// ImuFactor 因子 表征的两帧激光帧之间的imu积分变换量
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因子到因子图
// add imu bias between factor
// 添加imu偏置因子，前一帧偏置B(key - 1)，当前帧偏置B(key)，观测值，噪声协方差；deltaTij()是积分段的时间
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
添加位姿因子到因子图：根据位姿准不准，它添加的协方差是不一样的
// add pose factor
// 添加位姿因子
// 转到imu坐标系，只有平移
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
// 先验因子 此处定义了位姿因子的模型值curPose来自 mapoptimization
// 根据位姿准不准，它添加的协方差是不一样的
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
prevState_前一帧状态，prevBias_前一帧偏执，这里前一帧是指激光雷达，预测出当前时刻的数据
现在呢，从上一帧到当前帧之间的IMU有了，上一帧状态量也有，当前帧输入的里程计也有，那么就可以获取图优化之后当前帧的一个初始预测值了：
// insert predicted values
// 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态 （此处封装了成一个轮子 其内部如何实现预积分的我也不清楚）
// 那么此处得到的是依据上一时刻数据（prevState,prevBias）得到的当前时刻的数据
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
graphValues状态信息赋值
// 变量节点赋初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
优化
// optimize
optimizer.update(graphFactors, graphValues);
optimizer.update();
重置
// 重置
graphFactors.resize(0);
graphValues.clear();
优化结果result
// 优化结果
gtsam::Values result = optimizer.calculateEstimate();
用当前帧位姿 速度更新上一帧位姿 速度，下个循环要用：
// 更新当前帧位姿 速度
// 此处的优化结果 结合了扫描匹配后 经过因子图优化的激光里程计的数据
// 同时利用上一帧和两帧之间Imu预积分数据 进行一次因子图优化
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));
// Reset the optimization preintegration object.
// 重置预积分器 设置新的偏置 这样下一帧激光里程计进来的时候 预积分量就是两帧之间的增量
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// imu因子图优化的结果 速度过大或者偏置过大 认为失败
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
4.2.构建用于imu里程计的预积分器
从imuQueImu队列把当前帧之前的imu数据都给直接剔除掉，仅保留当前帧之后的imu数据
// 优化过后 执行重传播 优化更新了Imu的偏置
// 用最新的偏置重新计算当前激光里程计时刻之后的Imu预积分 这个预积分用于计算每时刻的位姿
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// 从imu队列中删除当前激光里程计时刻之前的imu数据
double lastImuQT = -1;
//注意，这里是要“删除”当前帧“之前”的imu数据，是想根据当前帧“之后”的累积递推。
//前面imuIntegratorOpt_做的事情是，“提取”当前帧“之前”的imu数据，用两帧之间的imu数据进行积分，处理过的数据就弹出来。
//因此，新到一帧激光帧里程计数据时，imuQueOpt队列变化如下：
//当前帧之前的数据被提出来做积分，用一个删一个（这样下一帧到达后，队列中就不会有现在这帧之前的数据了）
//那么在更新完以后，imuQueOpt队列不再变化，剩下的原始imu数据用作下一次优化时的数据。
//而imuQueImu队列则是把当前帧之前的imu数据都给直接剔除掉，仅保留当前帧之后的imu数据，
//用作两帧lidar里程计到达时刻之间发布的imu增量式里程计的预测。
//imuQueImu和imuQueOpt的区别要明确,imuIntegratorImu_和imuIntegratorOpt_的区别也要明确,见imuhandler中的注释
// imuQueImu 当前帧雷达之前的imu数据都给直接剔除掉，仅保留当前帧之后的imu数据，
// 用这之后imu的数据，计算相邻两个imu之间的imu增量式里程计
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
对剩余的imu数据计算预积分
// 对剩余的imu数据计算预积分
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
// 传入状态 重置预积分器和最新的偏置
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
// 计算预积分
// 利用imuQueImu中的数据进行预积分 主要区别在于上一行更新了bias
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_,(之前用来计算的是imuIntegratorOpt_,）
//注意加入了上一步算出的dt
//结果被存放在imuIntegratorImu_中
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;
//设置成True，用来通知另一个负责发布imu里程计的回调函数imuHandler“可以发布了”
doneFirstOpt = true;
}
/**
再进入imuHandler之前，插播一个key==100
4.3 每100个激光雷达里程计，重置优化器（与初始化时对比）
// 每隔100帧激光里程计，重置 ISAM2 优化器，保证优化效率
if (key == 100)
{
// get updated noise before reset
// 前一帧的位姿、速度、偏置噪声模型
//保存最后的噪声值
// 将上一imu原始时刻 预积分得到的 边界方差（边缘分布） 作为新模型的起始协方差
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)));
// reset graph
// 重置isam2优化器
resetOptimization();
// 添加位姿先验因子，用前一帧的值初始化
//重置之后还有类似与初始化的过程 区别在于噪声值不同
//prevPose_，prevVel_，priorBias三项，也是上一时刻得到的，
//（初始时刻是lidar里程计的pose直接用lidar2IMU变量转到imu坐标系下，而此处则是通过上一时刻，即接下来的后续优化中得到）
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;
}
五 imu里程计回调函数imuHandler（）
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数据到imuQueOpt与imuQueImu队列
// 添加当前帧imu数据到队列
// 两个双端队列分别装着优化前后的imu数据
imuQueOpt.push_back(thisImu); //用于预测每个时刻（IMU频率）imu里程计 用来执行预积分和位姿变换的优化
imuQueImu.push_back(thisImu); //用于因子图优化 用来更新最新imu状态
确保更新了上一帧（激光里程计帧）的状态
// 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分已经被重新计算
// 这里需要先在odomhandler中优化一次后再进行该函数后续的工作
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
//lastImuT_imu变量初始被赋值为-1
// 获得时间间隔, 第一次为1/500,之后是两次imuTime间的差
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
// 更新上一帧的时间
lastImuT_imu = imuTime;
imu预积分器添加最新一帧imu数据
// 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预计分量，得到当前时刻的状态
// 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态 imu_incremental
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
发布imu里程计
// 发布imu里程计（转到lidar系，与激光里程计同一个系）
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame; //odom坐标系
odometry.child_frame_id = "odom_imu";
预测值currentState获得imu位姿, 再由imu坐标变换到雷达坐标, 获得雷达位姿
// transform imu pose to ldiar
//预测值currentState获得imu位姿, 再由imu到雷达变换, 获得雷达位姿
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
// 将中间坐标系的姿态转移的真正的lidar坐标系下
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
//在更新的过程中不用担心会被imuHandler中的imuIntegratorImu_->integrateMeasurement给顶掉，
//这是因为imuHandler要根据doneFirstOpt来检查odometryHandler是不是已经更新完bias了。
//因为更新并不是实时的，而是一帧激光数据到了才更新。
//可是下一帧激光并没有到，但是在此期间imu增量式里程计还是得照常发布，
//如果当前帧已经更新完bias了，然后就可以直接利用这个bias计算后面新到的ImuIntegratorImu_，
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();
//imu_incremental
pubImuOdometry.publish(odometry);
}
// 这里留疑问，本cpp读完后补充：
// 为什么currentState获得的是imu的位姿？原始imu数据难道不是先转换成雷达坐标系下的数据（this imu）才再送到imu预积分器中吗？
//答： 在之前的优化函数odometryHandler中，thisIMU是直接从imuQueOpt中取值.
//而imuQueOpt中的内容，是已经从imu原始测量数据转换到了lidar"中间系"系下（在本函数第二行）。离真正的雷达系还差了一个平移
//odometryHandler函数中根据prevPose_ = lidarPose.compose(lidar2Imu)得到激光帧先验位姿(lidarPose)转换到imu系下,（相当于从真正的雷达系扭到上面的中间系中）
//作为初值构建了因子进行优化；
//在其imuIntegratorOpt_->integrateMeasurement中得到的应该是dt之间的预积分量，
//由于其处在循环中，因此是在递推累积计算两帧之间的预积分量。
//（相当于是每到一帧，就把二者之间的预积分量计算一遍，并且优化一遍，存储进imuIntegratorOpt_）中。
//因为本函数为imu回调函数，每收到一个imu数据，当之前因子图算完的情况下，
//在imuIntegratorImu_的基础上继续递推新收到的imu数据，并且进行预测。
//最后把imu再转回lidar系下进行发布。
//注意：这里发布的是两帧之间的“增量”imu里程计信息，
//imuIntegratorImu_本身是个积分器，只有两帧之间的预积分，但是发布的时候发布的实际是结合了前述里程计本身有的位姿
//如这个predict里的prevStateOdom:
//currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
//关于imuIntegratorImu_在两个回调函数中都进行integrateMeasurement操作，之间是否会有冲突呢？
//我觉得关键在于odometryHandler中有一句：imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom)，
//在imuIntegratorOpt_优化完两帧imu数据之后，imuIntegratorImu_直接把积分和bias给reset掉，
//然后开始根据imuIntegratorOpt_优化出的bias来更新imuIntegratorImu_。
//imuIntegratorImu_和imuIntegratorOpt_的区别在于，opt存储的是新到一帧，和上一帧之间的预积分量，作为约束，执行优化。
//优化后，imuIntegratorImu_利用新的bias，在新到的这一帧的基础上，递推“之后”的预积分量。
//（绝对不能把imuIntegratorOpt_和imuIntegratorImu_理解为同一批imu数据在优化前后的不同值）
373
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
