LIO-SAM 源码解析（四）：imuPreintegration.cpp 1. 代码流程2. 功能说明这个cpp文件主要有两个类一个叫IMUPreintegration类一个叫TransformFusion类。现在我们分开讲先说IMUPreintegration类。关于IMU原始数据送入imuhandle中2.1. imuhandleimu原始数据会先被坐标系转换通过调用头文件里的imuConverter函数转换到一个“雷达中间系”中其实和真正的雷达系差了一个平移。转换后会存入两个队列一个imuQueOpt队列一个imuQueImu队列。这两队列有什么区别和联系呢这个主要在另一个回调函数odometryHandler会被处理在那个地方我会讲。这里我们可以先理解为imuQueImu是真正我们要用的数据imuQueOpt是一个中间缓存的数据结构用来优化imu的bias之类的东西。在标志位doneFirstOpt为True的时候注意这个标志位这是一个很重要的变量之后会再提到每到一个imu数据就用imuIntegratorImu_这个预积分器把这一帧imu数据累积进去然后预测当前时刻的状态currentState imuIntegratorImu_-predict(prevStateOdom, prevBiasOdom);其中prevStateOdom就是之前系统所保持的状态。把currentState通过imu2Lidar从“中间系”给平移到真正的雷达系然后发布出去。发布的话题就叫odometry/imu_incremental这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。2.2. odomHandle这部分订阅的是/mapping/odometry_incremental这个话题是由mapOptmization.cpp发布的可以把它理解为激光里程计。同理也不要被incremental误导觉得好像是两帧激光之间的变换可不是这样的啊。它和imu里程计性质类似就是相对世界坐标系的位姿。2.2.1. 初始化系统从imuQueOpt队列里删掉当前这帧激光里程计时间上之前的数据然后把雷达的pose变换到“中间系”保存为prevPose。图优化部分加入priorVelpriorBias把两个预积分器imuIntegratorImu_imu积分器imuIntegratorOpt_opt积分器重置并设置bias。2.2.2. 清理缓存当处理了100帧激光雷达数据时就把优化器给重置一下用前面的先验协方差给reset一下。此处的重置与前面的重置是不同的前面是用先验值来初始化bias而此处是用轮子算出来的先验协方差optimizer.marginalCovariance来初始化。2.2.3. 正式处理假设数据如下分布之前imu数据 ——————第一帧开始——————第二帧开始————之后imu数据把“第一帧开始”——“第二帧开始”这个之间的imu数据拿出来送入opt积分器。这样得到二者之间的预积分构建imu因子。然后把 X(key-1) 到 X(key) 之间加入这个imu因子以及激光里程计提供的pose因子整体做一个优化。优化的结果就是bias以及“第二帧开始”这个时刻的系统位姿。把优化的结果主要是bias重置opt积分器和imu积分器。然后把当前帧上图的“第二帧开始”之前的数据给删掉用imu积分器从“第二帧开始”这里开始往后积分。我们需要明确一点在这个处理过程中imu队列也在持续的进数据即imuhandle中这里处理完那么就置donefirstTrue这样就可以无缝衔接接着在这里的基础上对imu积分器继续积分。在imuhandle中发布imu里程计在这里我们可以的出结果它并非是纯粹的imu里程计因为时不时是要加入激光里程计的信息做因子来优化得到imu的bias等信息的。2.3. TransformFusion类。2.3.1. lidarOdometryHandler这部分监听的是/mapping/odometry也就是激光雷达里程计这个回调函数比较特殊它并没有把雷达里程计的东西再像别的回调函数一样时刻存到什么队列里去。而是就保存当前的雷达里程计的数据到lidarOdomAffine里面把时间戳存到lidarOdomTime里面去。注意这里/mapping/odometry和/mapping/odometry_incremental有什么区别呢我认为本质上区别不大前者是激光里程计部分直接优化得到的激光位姿后者相当于激光的位姿经过旋转约束和z轴约束的限制以后又和原始imu信息里面的角度做了一个加权以后的位姿。2.3.2. imuOdometryHandler这部分监听的是/imu/odometry_incremental也就是上面我一直在说的imu里程计把imu里程计放到imuodomQueue里面保存然后把lidarOdomTime之前的数据扔掉用imu里程计的两时刻差异再加上lidarOdomAffine的基础进行发布。实际上/imu/odometry_incremental本身就是雷达里程计基础上imu数据上的发布而在现在这里也是雷达里程计在“imu里程计”上的一个“再次精修”。最后会发布两个内容一个是odometry/imu但是这个实际上是无人监听的我觉得作者主要是发布tf变换顺手给它发布了。当然我觉得用户可以监听这个数据我觉得这个应该是频率上最高的一个里程计数据了。另外还会发布一个path用来rviz显示名字叫lio-sam/imu/path。3. 代码#include utility.h #include gtsam/geometry/Rot3.h #include gtsam/geometry/Pose3.h #include gtsam/slam/PriorFactor.h #include gtsam/slam/BetweenFactor.h #include gtsam/navigation/GPSFactor.h #include gtsam/navigation/ImuFactor.h #include gtsam/navigation/CombinedImuFactor.h #include gtsam/nonlinear/NonlinearFactorGraph.h #include gtsam/nonlinear/LevenbergMarquardtOptimizer.h #include gtsam/nonlinear/Marginals.h #include gtsam/nonlinear/Values.h #include gtsam/inference/Symbol.h #include gtsam/nonlinear/ISAM2.h #include gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) class TransformFusion : public ParamServer { public: std::mutex mtx; ros::Subscriber subImuOdometry; ros::Subscriber subLaserOdometry; ros::Publisher pubImuOdometry; ros::Publisher pubImuPath; Eigen::Affine3f lidarOdomAffine; Eigen::Affine3f imuOdomAffineFront; Eigen::Affine3f imuOdomAffineBack; tf::TransformListener tfListener; tf::StampedTransform lidar2Baselink; double lidarOdomTime -1; dequenav_msgs::Odometry imuOdomQueue; /** * 构造函数 */ TransformFusion() { // 如果lidar系与baselink系不同激光系和载体系需要外部提供二者之间的变换关系 if(lidarFrame ! baselinkFrame) { try { // 等待3s tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0)); // lidar系到baselink系的变换 tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink); } catch (tf::TransformException ex) { ROS_ERROR(%s,ex.what()); } } // 订阅激光里程计来自mapOptimization subLaserOdometry nh.subscribenav_msgs::Odometry(lio_sam/mapping/odometry, 5, TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay()); // 订阅imu里程计来自IMUPreintegration subImuOdometry nh.subscribenav_msgs::Odometry(odomTopic_incremental, 2000, TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay()); // 发布imu里程计用于rviz展示 pubImuOdometry nh.advertisenav_msgs::Odometry(odomTopic, 2000); // 发布imu里程计轨迹 pubImuPath nh.advertisenav_msgs::Path (lio_sam/imu/path, 1); } /** * 里程计对应变换矩阵 */ Eigen::Affine3f odom2affine(nav_msgs::Odometry odom) { double x, y, z, roll, pitch, yaw; x odom.pose.pose.position.x; y odom.pose.pose.position.y; z odom.pose.pose.position.z; tf::Quaternion orientation; tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); return pcl::getTransformation(x, y, z, roll, pitch, yaw); } /** * 订阅激光里程计来自mapOptimization */ void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr odomMsg) { std::lock_guardstd::mutex lock(mtx); // 激光里程计对应变换矩阵 lidarOdomAffine odom2affine(*odomMsg); // 激光里程计时间戳 lidarOdomTime odomMsg-header.stamp.toSec(); } /** * 订阅imu里程计来自IMUPreintegration * 1、以最近一帧激光里程计位姿为基础计算该时刻与当前时刻间imu里程计增量位姿变换相乘得到当前时刻imu里程计位姿 * 2、发布当前时刻里程计位姿用于rviz展示发布imu里程计路径注只是最近一帧激光里程计时刻与当前时刻之间的一段 */ void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr odomMsg) { // 发布tfmap与odom系设为同一个系 static tf::TransformBroadcaster tfMap2Odom; static tf::Transform map_to_odom tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0)); tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg-header.stamp, mapFrame, odometryFrame)); std::lock_guardstd::mutex lock(mtx); // 添加imu里程计到队列 imuOdomQueue.push_back(*odomMsg); // 从imu里程计队列中删除当前最近的一帧激光里程计时刻之前的数据 if (lidarOdomTime -1) return; while (!imuOdomQueue.empty()) { if (imuOdomQueue.front().header.stamp.toSec() lidarOdomTime) imuOdomQueue.pop_front(); else break; } // 最近的一帧激光里程计时刻对应imu里程计位姿 Eigen::Affine3f imuOdomAffineFront odom2affine(imuOdomQueue.front()); // 当前时刻imu里程计位姿 Eigen::Affine3f imuOdomAffineBack odom2affine(imuOdomQueue.back()); // imu里程计增量位姿变换 Eigen::Affine3f imuOdomAffineIncre imuOdomAffineFront.inverse() * imuOdomAffineBack; // 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 当前时刻imu里程计位姿 Eigen::Affine3f imuOdomAffineLast lidarOdomAffine * imuOdomAffineIncre; float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw); // 发布当前时刻里程计位姿 nav_msgs::Odometry laserOdometry imuOdomQueue.back(); laserOdometry.pose.pose.position.x x; laserOdometry.pose.pose.position.y y; laserOdometry.pose.pose.position.z z; laserOdometry.pose.pose.orientation tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); pubImuOdometry.publish(laserOdometry); // 发布tf当前时刻odom与baselink系变换关系 static tf::TransformBroadcaster tfOdom2BaseLink; tf::Transform tCur; tf::poseMsgToTF(laserOdometry.pose.pose, tCur); if(lidarFrame ! baselinkFrame) tCur tCur * lidar2Baselink; tf::StampedTransform odom_2_baselink tf::StampedTransform(tCur, odomMsg-header.stamp, odometryFrame, baselinkFrame); tfOdom2BaseLink.sendTransform(odom_2_baselink); // 发布imu里程计路径注只是最近一帧激光里程计时刻与当前时刻之间的一段 static nav_msgs::Path imuPath; static double last_path_time -1; double imuTime imuOdomQueue.back().header.stamp.toSec(); // 每隔0.1s添加一个 if (imuTime - last_path_time 0.1) { last_path_time imuTime; geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.stamp imuOdomQueue.back().header.stamp; pose_stamped.header.frame_id odometryFrame; pose_stamped.pose laserOdometry.pose.pose; imuPath.poses.push_back(pose_stamped); // 删除最近一帧激光里程计时刻之前的imu里程计 while(!imuPath.poses.empty() imuPath.poses.front().header.stamp.toSec() lidarOdomTime - 1.0) imuPath.poses.erase(imuPath.poses.begin()); if (pubImuPath.getNumSubscribers() ! 0) { imuPath.header.stamp imuOdomQueue.back().header.stamp; imuPath.header.frame_id odometryFrame; pubImuPath.publish(imuPath); } } } }; class IMUPreintegration : public ParamServer { public: std::mutex mtx; // 订阅与发布 ros::Subscriber subImu; ros::Subscriber subOdometry; ros::Publisher pubImuOdometry; bool systemInitialized false; // 噪声协方差 gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2; gtsam::Vector noiseModelBetweenBias; // imu预积分器 gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_; gtsam::PreintegratedImuMeasurements *imuIntegratorImu_; // imu数据队列 std::dequesensor_msgs::Imu imuQueOpt; std::dequesensor_msgs::Imu imuQueImu; // imu因子图优化过程中的状态变量 gtsam::Pose3 prevPose_; gtsam::Vector3 prevVel_; gtsam::NavState prevState_; gtsam::imuBias::ConstantBias prevBias_; // imu状态 gtsam::NavState prevStateOdom; gtsam::imuBias::ConstantBias prevBiasOdom; bool doneFirstOpt false; double lastImuT_imu -1; double lastImuT_opt -1; // ISAM2优化器 gtsam::ISAM2 optimizer; gtsam::NonlinearFactorGraph graphFactors; gtsam::Values graphValues; const double delta_t 0; int key 1; // imu-lidar位姿变换 gtsam::Pose3 imu2Lidar gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); gtsam::Pose3 lidar2Imu gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); /** * 构造函数 */ IMUPreintegration() { // 订阅imu原始数据用下面因子图优化的结果施加两帧之间的imu预计分量预测每一时刻imu频率的imu里程计 subImu nh.subscribesensor_msgs::Imu (imuTopic, 2000, IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); // 订阅激光里程计来自mapOptimization用两帧之间的imu预计分量构建因子图优化当前帧位姿这个位姿仅用于更新每时刻的imu里程计以及下一次因子图优化 subOdometry nh.subscribenav_msgs::Odometry(lio_sam/mapping/odometry_incremental, 5, IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay()); // 发布imu里程计 pubImuOdometry nh.advertisenav_msgs::Odometry (odomTopic_incremental, 2000); // imu预积分的噪声协方差 boost::shared_ptrgtsam::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优化过程中发生退化则选择一个较大的协方差 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 } /** * 重置ISAM2优化器 */ 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; } /** * 重置参数 */ void resetParams() { lastImuT_imu -1; doneFirstOpt false; systemInitialized false; } /** * 订阅激光里程计来自mapOptimization * 1、每隔100帧激光里程计重置ISAM2优化器添加里程计、速度、偏置先验因子执行优化 * 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量用前一帧状态施加预积分量得到当前帧初始状态估计添加来自mapOptimization的当前帧位姿进行因子图优化更新当前帧状态 * 3、优化之后执行重传播优化更新了imu的偏置用最新的偏置重新计算当前激光里程计时刻之后的imu预积分这个预积分用于计算每时刻位姿 */ void odometryHandler(const nav_msgs::Odometry::ConstPtr odomMsg) { std::lock_guardstd::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)); // 0. 系统初始化第一帧 if (systemInitialized false) { // 重置ISAM2优化器 resetOptimization(); // 从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::PriorFactorgtsam::Pose3 priorPose(X(0), prevPose_, priorPoseNoise); graphFactors.add(priorPose); // 添加速度先验因子 prevVel_ gtsam::Vector3(0, 0, 0); gtsam::PriorFactorgtsam::Vector3 priorVel(V(0), prevVel_, priorVelNoise); graphFactors.add(priorVel); // 添加imu偏置先验因子 prevBias_ gtsam::imuBias::ConstantBias(); gtsam::PriorFactorgtsam::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(); // 重置优化之后的偏置 imuIntegratorImu_-resetIntegrationAndSetBias(prevBias_); imuIntegratorOpt_-resetIntegrationAndSetBias(prevBias_); key 1; systemInitialized true; return; } // 每隔100帧激光里程计重置ISAM2优化器保证优化效率 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::PriorFactorgtsam::Pose3 priorPose(X(0), prevPose_, updatedPoseNoise); graphFactors.add(priorPose); // 添加速度先验因子用前一帧的值初始化 gtsam::PriorFactorgtsam::Vector3 priorVel(V(0), prevVel_, updatedVelNoise); graphFactors.add(priorVel); // 添加偏置先验因子用前一帧的值初始化 gtsam::PriorFactorgtsam::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; } // 1. 计算前一帧与当前帧之间的imu预积分量用前一帧状态施加预积分量得到当前帧初始状态估计添加来自mapOptimization的当前帧位姿进行因子图优化更新当前帧状态 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_castconst 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偏置因子前一帧偏置当前帧偏置观测值噪声协方差deltaTij()是积分段的时间 graphFactors.add(gtsam::BetweenFactorgtsam::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::PriorFactorgtsam::Pose3 pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise); graphFactors.add(pose_factor); // 用前一帧的状态、偏置施加imu预计分量得到当前帧的状态 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.atgtsam::Pose3(X(key)); prevVel_ result.atgtsam::Vector3(V(key)); // 更新当前帧状态 prevState_ gtsam::NavState(prevPose_, prevVel_); // 更新当前帧imu偏置 prevBias_ result.atgtsam::imuBias::ConstantBias(B(key)); // 重置预积分器设置新的偏置这样下一帧激光里程计进来的时候预积分量就是两帧之间的增量 imuIntegratorOpt_-resetIntegrationAndSetBias(prevBias_); // imu因子图优化结果速度或者偏置过大认为失败 if (failureDetection(prevVel_, prevBias_)) { // 重置参数 resetParams(); return; } // 2. 优化之后执行重传播优化更新了imu的偏置用最新的偏置重新计算当前激光里程计时刻之后的imu预积分这个预积分用于计算每时刻位姿 prevStateOdom prevState_; prevBiasOdom prevBias_; // 从imu队列中删除当前激光里程计时刻之前的imu数据 double lastImuQT -1; while (!imuQueImu.empty() ROS_TIME(imuQueImu.front()) currentCorrectionTime - delta_t) { lastImuQT ROS_TIME(imuQueImu.front()); imuQueImu.pop_front(); } // 对剩余的imu数据计算预积分 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; doneFirstOpt true; } /** * imu因子图优化结果速度或者偏置过大认为失败 */ bool failureDetection(const gtsam::Vector3 velCur, const gtsam::imuBias::ConstantBias biasCur) { Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); if (vel.norm() 30) { ROS_WARN(Large velocity, reset IMU-preintegration!); return true; } Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z()); Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z()); if (ba.norm() 1.0 || bg.norm() 1.0) { ROS_WARN(Large bias, reset IMU-preintegration!); return true; } return false; } /** * 订阅imu原始数据 * 1、用上一帧激光里程计时刻对应的状态、偏置施加从该时刻开始到当前时刻的imu预计分量得到当前时刻的状态也就是imu里程计 * 2、imu里程计位姿转到lidar系发布里程计 */ void imuHandler(const sensor_msgs::Imu::ConstPtr imu_raw) { std::lock_guardstd::mutex lock(mtx); // imu原始测量数据转换到lidar系加速度、角速度、RPY sensor_msgs::Imu thisImu imuConverter(*imu_raw); // 添加当前帧imu数据到队列 imuQueOpt.push_back(thisImu); imuQueImu.push_back(thisImu); // 要求上一次imu因子图优化执行成功确保更新了上一帧激光里程计帧的状态、偏置预积分重新计算了 if (doneFirstOpt false) return; 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); // 发布imu里程计转到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); } }; 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); ros::MultiThreadedSpinner spinner(4); spinner.spin(); return 0; }参考文献LIOSAM代码架构 - 知乎SLAM学习笔记二十LIO-SAM流程及代码详解最全_zkk9527的博客-CSDN博客_lio-sam