LIO-SAM前端中基于IMU预积分的前端代码详解
文章目录
- 第一次进入imuHandler函数
- 跳出 imuHandler 进入 odometryHandler
- 初始化系统
- 开始处理优化
- 优化后开始位姿传播
- 回到imuHandler函数
- 关于高频发布odom时两个函数的配合
- odometryHandler中约束过多的处理
这里将通过代码解析预积分这个节点的运行顺行
第一次进入imuHandler函数
- 首先，该节点自己的main函数
int main(int argc, char** argv)
{
ros::init(argc, argv, "roboat_loam");
IMUPreintegration ImuP;
TransformFusion TF;
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
//开4个线程
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
运行IMUPreintegration ImuP里面的构造函数中的回调函数
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
回调函数只有这两个，第一个订阅的话题是IMU的原始数据，第二个订阅的话题是lidar里程计的位姿
- 先讲订阅IMU原始数据的回调函数&IMUPreintegration::imuHandler
- 先把IMU的消息做一个旋转变换，把IMU数据旋转到前左上坐标系下
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
具体操作代码在这里
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
sensor_msgs::Imu imu_out = imu_in;
// 这里把imu的数据旋转到前左上坐标系下，可以参考https://github.com/TixiaoShan/LIO-SAM/issues/6
// rotate acceleration
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
// rotate gyroscope
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
// rotate roll pitch yaw
// 这是一个九轴imu，因此还会有姿态信息
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
// 简单校验一下结果
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
ros::shutdown();
}
return imu_out;
}
其中的 extRot 是params中的参数，具体长这样
extrinsicRot: [1, 0, 0,
0, 1, 0,
0, 0, 1]
- 然后将数据存进两个队列中，一个用于执行预积分和位姿优化，一个用于更新IMU状态，第二个会用到第一个的优化结果，后面会讲
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
- 第一次进这个函数的时候是没有发生过优化的，此时会跳出该函数，具体什么时候会接着运行后面会顺下来的
if (doneFirstOpt == false)
return;
跳出 imuHandler 进入 odometryHandler
- 这个时候就跑去运行第二个回调函数了 &IMUPreintegration::odometryHandler
这个回调函数是订阅地图优化节点的增量里程计信息
- 保存订阅的位姿的时间戳，用于后面的IMU数据时间对齐
double currentCorrectionTime = ROS_TIME(odomMsg);
- 获取里程计信息并转成gtsam的格式
// 确保imu队列中有数据
if (imuQueOpt.empty())
return;
// 获取里程记位姿
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的格式
gtsam::Pose3 lidarPose = gtsam::Pose
初始化系统
- 第一次进入时需要初始化系统
- 优化问题复位
void resetOptimization()
{
// gtsam初始化
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1;//差值大于0.1需要重新线性化
optParameters.relinearizeSkip = 1; //每当有1个值需要重新线性化时，对贝叶斯树进行更新
optimizer = gtsam::ISAM2(optParameters);
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}
- 将当前里程计消息前的imu信息全部扔掉
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
- 初始化位姿、速度和零偏，并加入因子图中
// 将lidar的位姿转移到imu坐标系下
prevPose_ = lidarPose.compose(lidar2Imu);
// 设置其初始位姿和置信度
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
// 约束加入到因子中
graphFactors.add(priorPose);
// initial velocity
// 初始化速度，这里就直接赋0了
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
// 将对速度的约束也加入到因子图中
graphFactors.add(priorVel);
// initial bias
// 初始化零偏
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_);
- 约束和状态量更新进isam优化器
optimizer.update(graphFactors, graphValues);
- 使用初始化零偏对预计分接口进行初始化
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
这两个变量是gtsam库自带的类型，具体为，虽然预积分原理复杂，但是这里封装好了，预计分的操作都在库里面进行，我们只关注结果即可
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
上面的位姿、速度、零偏的置信度在这里设置
// 初始位姿置信度设置比较高
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
开始处理优化
- 至此完成了系统的初始化，其实主要还是对优化库的初始变量的设置
- 对两帧间的imu做积分，将原始imu数据放入库的预积分接口进行操作即可
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
// 将imu消息取出来
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
// 时间上小于当前lidar位姿的都取出来
if (imuTime < currentCorrectionTime - delta_t)
{
// 计算两个imu量之间的时间差
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
// 调用预积分接口将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);
// 记录当前imu时间
lastImuT_opt = imuTime;
imuQueOpt.pop_front();
}
else
break;
}
- 将预计分约束、零偏约束、位姿约束加入因子图中
// add imu factor to graph
// 两帧间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);
// add imu bias between factor
// 零偏的约束，两帧间零偏相差不会太大，因此使用常量约束
// 前一帧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)));
// add pose factor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
// lidar位姿补偿到imu坐标系下，同时根据是否退化选择不同的置信度，作为这一帧的先验估计
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
// 加入因子图中去
graphFactors.add(pose_factor);
- 根据上异常时刻的状态，结合预积分的结果，对当前状态进行预测，并将预测值作为初始值插入因子图中
// 根据上一时刻的状态，结合预积分结果，对当前状态进行预测
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
// 预测量作为初始值插入因子图中
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
- 开始执行优化
// 执行优化
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
- 获取优化有的当前状态作为当前帧的最佳估计
gtsam::Values result = optimizer.calculateEstimate();
// 获取优化后的当前状态作为当前帧的最佳估计
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
prevState_ = gtsam::NavState(prevPose_, prevVel_);
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
- 当前约束任务已经完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先决条件
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// 一个简单的失败检测
if (failureDetection(prevVel_, prevBias_))
{
// 状态异常就直接复位了
resetParams();
return;
}
优化后开始位姿传播
思路描述：假设当前时间是k+n，第k时间戳的关键帧是被优化过，即认为其位姿是准确的，同时这里假设短时间内imu漂移不会太大，此时第k+n时间的位姿 = 积分k~k+n的位姿 +第k+n的位姿，这样就可以以更快的频率发布位姿，同时结果也比较准确，因为优化的位姿发布可能只有10 ~ 30 Hz ，这样用imu的频率发布有100Hz，里程计发布在 imuHandler 函数中
// 优化之后，根据最新的imu状态进行传播
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
double lastImuQT = -1;
// 首先把lidar帧之前的imu状态全部弹出去
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
// 如果有新于lidar状态时刻的imu
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
// 这个预积分变量复位
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
// 然后把剩下的imu状态重新积分
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;
}
}
回到imuHandler函数
- 每来一个imu值就加入预积分状态中
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// integrate this single imu message
// 每来一个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);
- 根据这个预测值预测最新的状态
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
注意这里的 prevStateOdom, prevBiasOdom 是 odometryHandler 函数优化进行传播赋值的，即第k时刻的优化值
- 发布位姿
// publish odometry
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
// 将这个状态转到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);
关于高频发布odom时两个函数的配合
odometryHandler 和 imuHandler 至始至终都在处理，以下两个变量
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
两个区别在于
imuQueOpt 是用于处理两帧间积分的，而 imuQueImu 是用于处理优化后但还没到新关键帧来时的中间位姿积分，来进行高频发布odom计算用的
- 对这两个变量的实际操作都在 odometryHandler 中，其中 imuQueImu 在 odometryHandler函数 中计算预积分是因为两个函数在轮流调用，这样两个函数中都有预积分，而且都是用相同的接口 imuIntegratorImu_->integrateMeasurement 这样就可以确保积分是不间断的
- 而 imuHandler 函数只需要接着预积分即可，算完直接发布就行，因为是用同样的接口，有一部分预积分是在 odometryHandler 中算的
- 注意预积分接口有两个
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
-
imuIntegratorOpt_ 是在优化前进行预积分使用的，其使用的数据是imuQueOpt ，这个变量的预测结果是给优化当初始值的，预积分值给到因子图进行预积分约束
-
优化后将结果中的零偏 prevBias_ 给到 imuIntegratorImu_这个预积分接口进行复位，然后对 imuQueImu 数据进行预积分运算，再在 imuHandler 中用这个复位过的 imuIntegratorImu_预积分接口进行计算，最后再根据优化后的结果prevState_ ，prevBias_对当前状态进行预测，然后就能以高频发布比较准确的位姿了
odometryHandler中约束过多的处理
- 当isam优化器中加入了较多的约束后，为了避免运算时间变长，就直接清空，当有100帧时直接清空过去的所有约束，但是会保存最新时刻的位姿、速度、零偏及其协方差矩阵
if (key == 100)
{
// get updated noise before reset
// 取出最新时刻位姿 速度 零偏的协方差矩阵
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
// 复位整个优化问题
resetOptimization();
// add pose
// 将最新的位姿，速度，零偏以及对应的协方差矩阵加入到因子图中
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// add velocity
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// add bias
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}