• LIO-SAM源码解析（四）：imuPreintegration.cpp

    1. 代码流程

    2. 功能说明

    这个cpp文件主要有两个类，一个叫IMUPreintegration类，一个叫TransformFusion类。

    现在我们分开讲，先说IMUPreintegration类。

    关于IMU原始数据，送入imuhandle中：

    2.1. imuhandle

    imu原始数据，会先被坐标系转换，通过调用头文件里的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。图优化部分，加入乱七八糟各种东西，priorVel，priorBias，把两个预积分器imuIntegratorImu_，imuIntegratorOpt_给reset一下。（之后简称imu积分器和opt积分器）

    这两个预积分器opt积分器和imu积分器有什么区别呢？马上就要讲，在1.2.3部分。

    2.2.2. 清理缓存

    100帧激光里程计数据了，就把优化器给reset一下（用前面的先验协方差给reset一下）。注意，1.2.1和1.2.2的主要区别在于，1.2.1中的乱七八糟协方差数据，是用构造函数中写入的一大堆（我认为是经验值），而1.2.2这里的协方差，用的是optimizer.marginalCovariance这个轮子算出来的先验协方差。

    2.2.3. 正式处理

    假设数据如下分布：

    之前imu数据 ——————第一帧开始——————第二帧开始————之后imu数据

    把“第一帧开始”——“第二帧开始”这个之间的imu数据拿出来，送入opt积分器。这样得到二者之间的预积分，构建imu因子。

    然后把Xkey-1 到Xkey之间，加入这个imu因子以及 激光里程计提供的pose因子，整体做一个优化。优化的结果就是bias，以及“第二帧开始”这个时刻的系统位姿。

    把优化的结果（主要是bias），重置opt积分器和imu积分器。 然后把当前帧（上图的“第二帧开始”）之前的数据给删掉，用imu积分器，从“第二帧开始”这里开始往后积分。（我们需要明确一点，在这个处理过程中，imu队列也在持续的进数据，（即1.1的imuhandle中）），这里处理完，那么就置donefirst=True，这样1.1.3部分，就可以无缝衔接接着在这里的基础上对imu积分器继续积分。（可以看出，这点处理，Tixiaoshan还是做的比较牛的）

     回顾：在1.1.3部分，发布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. 代码

    1. #include "utility.h"
    2.  
    3. #include 
    4. #include 
    5. #include 
    6. #include 
    7. #include 
    8. #include 
    9. #include 
    10. #include 
    11. #include 
    12. #include 
    13. #include 
    14. #include 
    15.  
    16. #include 
    17. #include 
    18.  
    19. using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
    20. using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
    21. using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
    22.  
    23. class TransformFusion : public ParamServer
    24. {
    25. public:
    26.     std::mutex mtx;
    27.  
    28.     ros::Subscriber subImuOdometry;
    29.     ros::Subscriber subLaserOdometry;
    30.  
    31.     ros::Publisher pubImuOdometry;
    32.     ros::Publisher pubImuPath;
    33.  
    34.     Eigen::Affine3f lidarOdomAffine;
    35.     Eigen::Affine3f imuOdomAffineFront;
    36.     Eigen::Affine3f imuOdomAffineBack;
    37.  
    38.     tf::TransformListener tfListener;
    39.     tf::StampedTransform lidar2Baselink;
    40.  
    41.     double lidarOdomTime = -1;
    42.     deque imuOdomQueue;
    43.  
    44.     /**
    45.      * 构造函数
    46.     */
    47.     TransformFusion()
    48.     {
    49.         // 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的变换关系
    50.         if(lidarFrame != baselinkFrame)
    51.         {
    52.             try
    53.             {
    54.                 // 等待3s
    55.                 tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
    56.                 // lidar系到baselink系的变换
    57.                 tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
    58.             }
    59.             catch (tf::TransformException ex)
    60.             {
    61.                 ROS_ERROR("%s",ex.what());
    62.             }
    63.         }
    64.  
    65.         // 订阅激光里程计，来自mapOptimization
    66.         subLaserOdometry = nh.subscribe("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
    67.         // 订阅imu里程计，来自IMUPreintegration
    68.         subImuOdometry   = nh.subscribe(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());
    69.  
    70.         // 发布imu里程计，用于rviz展示
    71.         pubImuOdometry   = nh.advertise(odomTopic, 2000);
    72.         // 发布imu里程计轨迹
    73.         pubImuPath       = nh.advertise    ("lio_sam/imu/path", 1);
    74.     }
    75.  
    76.     /**
    77.      * 里程计对应变换矩阵
    78.     */
    79.     Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
    80.     {
    81.         double x, y, z, roll, pitch, yaw;
    82.         x = odom.pose.pose.position.x;
    83.         y = odom.pose.pose.position.y;
    84.         z = odom.pose.pose.position.z;
    85.         tf::Quaternion orientation;
    86.         tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
    87.         tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
    88.         return pcl::getTransformation(x, y, z, roll, pitch, yaw);
    89.     }
    90.  
    91.     /**
    92.      * 订阅激光里程计，来自mapOptimization
    93.     */
    94.     void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    95.     {
    96.         std::lock_guard lock(mtx);
    97.         // 激光里程计对应变换矩阵
    98.         lidarOdomAffine = odom2affine(*odomMsg);
    99.         // 激光里程计时间戳
    100.         lidarOdomTime = odomMsg->header.stamp.toSec();
    101.     }
    102.  
    103.     /**
    104.      * 订阅imu里程计，来自IMUPreintegration
    105.      * 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
    106.      * 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
    107.     */
    108.     void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    109.     {
    110.         // 发布tf，map与odom系设为同一个系
    111.         static tf::TransformBroadcaster tfMap2Odom;
    112.         static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
    113.         tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
    114.  
    115.         std::lock_guard lock(mtx);
    116.  
    117.         // 添加imu里程计到队列
    118.         imuOdomQueue.push_back(*odomMsg);
    119.  
    120.         // 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据
    121.         if (lidarOdomTime == -1)
    122.             return;
    123.         while (!imuOdomQueue.empty())
    124.         {
    125.             if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
    126.                 imuOdomQueue.pop_front();
    127.             else
    128.                 break;
    129.         }
    130.         // 最近的一帧激光里程计时刻对应imu里程计位姿
    131.         Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
    132.         // 当前时刻imu里程计位姿
    133.         Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
    134.         // imu里程计增量位姿变换
    135.         Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
    136.         // 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 = 当前时刻imu里程计位姿
    137.         Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
    138.         float x, y, z, roll, pitch, yaw;
    139.         pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
    140.         
    141.         // 发布当前时刻里程计位姿
    142.         nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
    143.         laserOdometry.pose.pose.position.x = x;
    144.         laserOdometry.pose.pose.position.y = y;
    145.         laserOdometry.pose.pose.position.z = z;
    146.         laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
    147.         pubImuOdometry.publish(laserOdometry);
    148.         // 发布tf，当前时刻odom与baselink系变换关系
    149.         static tf::TransformBroadcaster tfOdom2BaseLink;
    150.         tf::Transform tCur;
    151.         tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
    152.         if(lidarFrame != baselinkFrame)
    153.             tCur = tCur * lidar2Baselink;
    154.         tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
    155.         tfOdom2BaseLink.sendTransform(odom_2_baselink);
    156.  
    157.         // 发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
    158.         static nav_msgs::Path imuPath;
    159.         static double last_path_time = -1;
    160.         double imuTime = imuOdomQueue.back().header.stamp.toSec();
    161.         // 每隔0.1s添加一个
    162.         if (imuTime - last_path_time > 0.1)
    163.         {
    164.             last_path_time = imuTime;
    165.             geometry_msgs::PoseStamped pose_stamped;
    166.             pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
    167.             pose_stamped.header.frame_id = odometryFrame;
    168.             pose_stamped.pose = laserOdometry.pose.pose;
    169.             imuPath.poses.push_back(pose_stamped);
    170.             // 删除最近一帧激光里程计时刻之前的imu里程计
    171.             while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
    172.                 imuPath.poses.erase(imuPath.poses.begin());
    173.             if (pubImuPath.getNumSubscribers() != 0)
    174.             {
    175.                 imuPath.header.stamp = imuOdomQueue.back().header.stamp;
    176.                 imuPath.header.frame_id = odometryFrame;
    177.                 pubImuPath.publish(imuPath);
    178.             }
    179.         }
    180.     }
    181. };
    182.  
    183. class IMUPreintegration : public ParamServer
    184. {
    185. public:
    186.  
    187.     std::mutex mtx;
    188.  
    189.     // 订阅与发布
    190.     ros::Subscriber subImu;
    191.     ros::Subscriber subOdometry;
    192.     ros::Publisher pubImuOdometry;
    193.  
    194.     bool systemInitialized = false;
    195.  
    196.     // 噪声协方差
    197.     gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
    198.     gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
    199.     gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
    200.     gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
    201.     gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
    202.     gtsam::Vector noiseModelBetweenBias;
    203.  
    204.     // imu预积分器
    205.     gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
    206.     gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
    207.  
    208.     // imu数据队列
    209.     std::deque imuQueOpt;
    210.     std::deque imuQueImu;
    211.  
    212.     // imu因子图优化过程中的状态变量
    213.     gtsam::Pose3 prevPose_;
    214.     gtsam::Vector3 prevVel_;
    215.     gtsam::NavState prevState_;
    216.     gtsam::imuBias::ConstantBias prevBias_;
    217.  
    218.     // imu状态
    219.     gtsam::NavState prevStateOdom;
    220.     gtsam::imuBias::ConstantBias prevBiasOdom;
    221.  
    222.     bool doneFirstOpt = false;
    223.     double lastImuT_imu = -1;
    224.     double lastImuT_opt = -1;
    225.  
    226.     // ISAM2优化器
    227.     gtsam::ISAM2 optimizer;
    228.     gtsam::NonlinearFactorGraph graphFactors;
    229.     gtsam::Values graphValues;
    230.  
    231.     const double delta_t = 0;
    232.  
    233.     int key = 1;
    234.  
    235.     // imu-lidar位姿变换
    236.     gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
    237.     gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
    238.  
    239.     /**
    240.      * 构造函数
    241.     */
    242.     IMUPreintegration()
    243.     {
    244.         // 订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的imu预计分量，预测每一时刻（imu频率）的imu里程计
    245.         subImu      = nh.subscribe  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
    246.         // 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
    247.         subOdometry = nh.subscribe("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
    248.         
    249.         // 发布imu里程计
    250.         pubImuOdometry = nh.advertise (odomTopic+"_incremental", 2000);
    251.  
    252.         // imu预积分的噪声协方差
    253.         boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
    254.         p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
    255.         p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
    256.         p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
    257.         gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
    258.  
    259.         // 噪声先验
    260.         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
    261.         priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
    262.         priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
    263.         // 激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差
    264.         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
    265.         correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
    266.         noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
    267.         
    268.         // imu预积分器，用于预测每一时刻（imu频率）的imu里程计（转到lidar系了，与激光里程计同一个系）
    269.         imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
    270.         // imu预积分器，用于因子图优化
    271.         imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        
    272.     }
    273.  
    274.     /**
    275.      * 重置ISAM2优化器
    276.     */
    277.     void resetOptimization()
    278.     {
    279.         gtsam::ISAM2Params optParameters;
    280.         optParameters.relinearizeThreshold = 0.1;
    281.         optParameters.relinearizeSkip = 1;
    282.         optimizer = gtsam::ISAM2(optParameters);
    283.  
    284.         gtsam::NonlinearFactorGraph newGraphFactors;
    285.         graphFactors = newGraphFactors;
    286.  
    287.         gtsam::Values NewGraphValues;
    288.         graphValues = NewGraphValues;
    289.     }
    290.  
    291.     /**
    292.      * 重置参数
    293.     */
    294.     void resetParams()
    295.     {
    296.         lastImuT_imu = -1;
    297.         doneFirstOpt = false;
    298.         systemInitialized = false;
    299.     }
    300.  
    301.     /**
    302.      * 订阅激光里程计，来自mapOptimization
    303.      * 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化
    304.      * 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
    305.      * 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
    306.     */
    307.     void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    308.     {
    309.         std::lock_guard lock(mtx);
    310.         // 当前帧激光里程计时间戳
    311.         double currentCorrectionTime = ROS_TIME(odomMsg);
    312.  
    313.         // 确保imu优化队列中有imu数据进行预积分
    314.         if (imuQueOpt.empty())
    315.             return;
    316.  
    317.         // 当前帧激光位姿，来自scan-to-map匹配、因子图优化后的位姿
    318.         float p_x = odomMsg->pose.pose.position.x;
    319.         float p_y = odomMsg->pose.pose.position.y;
    320.         float p_z = odomMsg->pose.pose.position.z;
    321.         float r_x = odomMsg->pose.pose.orientation.x;
    322.         float r_y = odomMsg->pose.pose.orientation.y;
    323.         float r_z = odomMsg->pose.pose.orientation.z;
    324.         float r_w = odomMsg->pose.pose.orientation.w;
    325.         bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
    326.         gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
    327.  
    328.  
    329.         // 0. 系统初始化，第一帧
    330.         if (systemInitialized == false)
    331.         {
    332.             // 重置ISAM2优化器
    333.             resetOptimization();
    334.  
    335.             // 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据
    336.             while (!imuQueOpt.empty())
    337.             {
    338.                 if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
    339.                 {
    340.                     lastImuT_opt = ROS_TIME(&imuQueOpt.front());
    341.                     imuQueOpt.pop_front();
    342.                 }
    343.                 else
    344.                     break;
    345.             }
    346.             // 添加里程计位姿先验因子
    347.             prevPose_ = lidarPose.compose(lidar2Imu);
    348.             gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise);
    349.             graphFactors.add(priorPose);
    350.             // 添加速度先验因子
    351.             prevVel_ = gtsam::Vector3(0, 0, 0);
    352.             gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise);
    353.             graphFactors.add(priorVel);
    354.             // 添加imu偏置先验因子
    355.             prevBias_ = gtsam::imuBias::ConstantBias();
    356.             gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise);
    357.             graphFactors.add(priorBias);
    358.             // 变量节点赋初值
    359.             graphValues.insert(X(0), prevPose_);
    360.             graphValues.insert(V(0), prevVel_);
    361.             graphValues.insert(B(0), prevBias_);
    362.             // 优化一次
    363.             optimizer.update(graphFactors, graphValues);
    364.             graphFactors.resize(0);
    365.             graphValues.clear();
    366.             
    367.             // 重置优化之后的偏置
    368.             imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
    369.             imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
    370.             
    371.             key = 1;
    372.             systemInitialized = true;
    373.             return;
    374.         }
    375.  
    376.  
    377.         // 每隔100帧激光里程计，重置ISAM2优化器，保证优化效率
    378.         if (key == 100)
    379.         {
    380.             // 前一帧的位姿、速度、偏置噪声模型
    381.             gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
    382.             gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
    383.             gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
    384.             // 重置ISAM2优化器
    385.             resetOptimization();
    386.             // 添加位姿先验因子，用前一帧的值初始化
    387.             gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise);
    388.             graphFactors.add(priorPose);
    389.             // 添加速度先验因子，用前一帧的值初始化
    390.             gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise);
    391.             graphFactors.add(priorVel);
    392.             // 添加偏置先验因子，用前一帧的值初始化
    393.             gtsam::PriorFactor priorBias(B(0), prevBias_, updatedBiasNoise);
    394.             graphFactors.add(priorBias);
    395.             // 变量节点赋初值，用前一帧的值初始化
    396.             graphValues.insert(X(0), prevPose_);
    397.             graphValues.insert(V(0), prevVel_);
    398.             graphValues.insert(B(0), prevBias_);
    399.             // 优化一次
    400.             optimizer.update(graphFactors, graphValues);
    401.             graphFactors.resize(0);
    402.             graphValues.clear();
    403.  
    404.             key = 1;
    405.         }
    406.  
    407.  
    408.         // 1. 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
    409.         while (!imuQueOpt.empty())
    410.         {
    411.             // 提取前一帧与当前帧之间的imu数据，计算预积分
    412.             sensor_msgs::Imu *thisImu = &imuQueOpt.front();
    413.             double imuTime = ROS_TIME(thisImu);
    414.             if (imuTime < currentCorrectionTime - delta_t)
    415.             {
    416.                 // 两帧imu数据时间间隔
    417.                 double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
    418.                 // imu预积分数据输入：加速度、角速度、dt
    419.                 imuIntegratorOpt_->integrateMeasurement(
    420.                         gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
    421.                         gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
    422.                 
    423.                 lastImuT_opt = imuTime;
    424.                 // 从队列中删除已经处理的imu数据
    425.                 imuQueOpt.pop_front();
    426.             }
    427.             else
    428.                 break;
    429.         }
    430.         // 添加imu预积分因子
    431.         const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
    432.         // 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预计分量
    433.         gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
    434.         graphFactors.add(imu_factor);
    435.         // 添加imu偏置因子，前一帧偏置，当前帧偏置，观测值，噪声协方差；deltaTij()是积分段的时间
    436.         graphFactors.add(gtsam::BetweenFactor(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
    437.                          gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
    438.         // 添加位姿因子
    439.         gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
    440.         gtsam::PriorFactor pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
    441.         graphFactors.add(pose_factor);
    442.         // 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态
    443.         gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
    444.         // 变量节点赋初值
    445.         graphValues.insert(X(key), propState_.pose());
    446.         graphValues.insert(V(key), propState_.v());
    447.         graphValues.insert(B(key), prevBias_);
    448.         // 优化
    449.         optimizer.update(graphFactors, graphValues);
    450.         optimizer.update();
    451.         graphFactors.resize(0);
    452.         graphValues.clear();
    453.         // 优化结果
    454.         gtsam::Values result = optimizer.calculateEstimate();
    455.         // 更新当前帧位姿、速度
    456.         prevPose_  = result.at(X(key));
    457.         prevVel_   = result.at(V(key));
    458.         // 更新当前帧状态
    459.         prevState_ = gtsam::NavState(prevPose_, prevVel_);
    460.         // 更新当前帧imu偏置
    461.         prevBias_  = result.at(B(key));
    462.         // 重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量
    463.         imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
    464.  
    465.         // imu因子图优化结果，速度或者偏置过大，认为失败
    466.         if (failureDetection(prevVel_, prevBias_))
    467.         {
    468.             // 重置参数
    469.             resetParams();
    470.             return;
    471.         }
    472.  
    473.  
    474.         // 2. 优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
    475.         prevStateOdom = prevState_;
    476.         prevBiasOdom  = prevBias_;
    477.         // 从imu队列中删除当前激光里程计时刻之前的imu数据
    478.         double lastImuQT = -1;
    479.         while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
    480.         {
    481.             lastImuQT = ROS_TIME(&imuQueImu.front());
    482.             imuQueImu.pop_front();
    483.         }
    484.         // 对剩余的imu数据计算预积分
    485.         if (!imuQueImu.empty())
    486.         {
    487.             // 重置预积分器和最新的偏置
    488.             imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
    489.             // 计算预积分
    490.             for (int i = 0; i < (int)imuQueImu.size(); ++i)
    491.             {
    492.                 sensor_msgs::Imu *thisImu = &imuQueImu[i];
    493.                 double imuTime = ROS_TIME(thisImu);
    494.                 double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
    495.  
    496.                 imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
    497.                                                         gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
    498.                 lastImuQT = imuTime;
    499.             }
    500.         }
    501.  
    502.         ++key;
    503.         doneFirstOpt = true;
    504.     }
    505.  
    506.     /**
    507.      * imu因子图优化结果，速度或者偏置过大，认为失败
    508.     */
    509.     bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
    510.     {
    511.         Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
    512.         if (vel.norm() > 30)
    513.         {
    514.             ROS_WARN("Large velocity, reset IMU-preintegration!");
    515.             return true;
    516.         }
    517.  
    518.         Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
    519.         Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
    520.         if (ba.norm() > 1.0 || bg.norm() > 1.0)
    521.         {
    522.             ROS_WARN("Large bias, reset IMU-preintegration!");
    523.             return true;
    524.         }
    525.  
    526.         return false;
    527.     }
    528.  
    529.     /**
    530.      * 订阅imu原始数据
    531.      * 1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计
    532.      * 2、imu里程计位姿转到lidar系，发布里程计
    533.     */
    534.     void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
    535.     {
    536.         std::lock_guard lock(mtx);
    537.         // imu原始测量数据转换到lidar系，加速度、角速度、RPY
    538.         sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
    539.  
    540.         // 添加当前帧imu数据到队列
    541.         imuQueOpt.push_back(thisImu);
    542.         imuQueImu.push_back(thisImu);
    543.  
    544.         // 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分重新计算了
    545.         if (doneFirstOpt == false)
    546.             return;
    547.  
    548.         double imuTime = ROS_TIME(&thisImu);
    549.         double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
    550.         lastImuT_imu = imuTime;
    551.  
    552.         // imu预积分器添加一帧imu数据，注：这个预积分器的起始时刻是上一帧激光里程计时刻
    553.         imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
    554.                                                 gtsam::Vector3(thisImu.angular_velocity.x,    thisImu.angular_velocity.y,    thisImu.angular_velocity.z), dt);
    555.  
    556.         // 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态
    557.         gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
    558.  
    559.         // 发布imu里程计（转到lidar系，与激光里程计同一个系）
    560.         nav_msgs::Odometry odometry;
    561.         odometry.header.stamp = thisImu.header.stamp;
    562.         odometry.header.frame_id = odometryFrame;
    563.         odometry.child_frame_id = "odom_imu";
    564.  
    565.         // 变换到lidar系
    566.         gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
    567.         gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
    568.  
    569.         odometry.pose.pose.position.x = lidarPose.translation().x();
    570.         odometry.pose.pose.position.y = lidarPose.translation().y();
    571.         odometry.pose.pose.position.z = lidarPose.translation().z();
    572.         odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
    573.         odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
    574.         odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
    575.         odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
    576.         
    577.         odometry.twist.twist.linear.x = currentState.velocity().x();
    578.         odometry.twist.twist.linear.y = currentState.velocity().y();
    579.         odometry.twist.twist.linear.z = currentState.velocity().z();
    580.         odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
    581.         odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
    582.         odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
    583.         pubImuOdometry.publish(odometry);
    584.     }
    585. };
    586.  
    587.  
    588. int main(int argc, char** argv)
    589. {
    590.     ros::init(argc, argv, "roboat_loam");
    591.     
    592.     IMUPreintegration ImuP;
    593.  
    594.     TransformFusion TF;
    595.  
    596.     ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
    597.     
    598.     ros::MultiThreadedSpinner spinner(4);
    599.     spinner.spin();
    600.     
    601.     return 0;
    602. }

    参考文献

    LIOSAM代码架构 - 知乎

    SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）_zkk9527的博客-CSDN博客_lio-sam 

  • 相关阅读:
    Android里面copy资源文件到目标目录中
    uniApp中 将2一张图片叠加到指定位置
    [hadoop全分布部署]虚拟机Hadoop集群配置/etc/hosts、配置无密码登录（SSH）
    【实战详解】如何快速搭建接口自动化测试框架？Python + Requests
    Autosar MCAL-ADC详解（一）-基于Tc27x的cfg软件
    Redis三种模式——主从复制，哨兵模式，集群
    「大屏」上车跑出加速度，哪些供应商在「领跑」娱乐中控
    【试题040】多个逻辑或例题2
    ERINE系列论文解读
    H - Hot Black Hot White（数论/取模运算）
  • 原文地址：https://blog.csdn.net/xhtchina/article/details/128158977