LIO-SAM 详读代码笔记--2.imuPreintegration

先把系统topic 数据流图搬过来，能更加清晰的理解这个模块。

图2.ros topic 发布与订阅关系图关系图（还是叫图2，因为是同一个图）

这个cpp文件主要有两个类，一个叫IMUPreintegration类，一个叫TransformFusion类。从数据流向和依赖程度的角度分析，应该先分析IMUPreintegration类，但是TransformFusion类写在该模块的前头，且这两个模块的数据流相互依赖，这里先分析TransformFusion类。

TransformFusion类

讲解之前，二话不说，先上该类的成员变量列表。

纵观TransformFusion类，该类只干一件事，就是融合lidar里程计(经过mapOptimization 优化过的)和imu 积分的里程计，以比lidar里程计更高的频率分布出去，一个是odometry/imu，一个是odom 到 baselink 的tf 关系，这个就是整个系统的主要输出。

主要的处理逻辑在imu 里程计的回调函数中，imuOdometryHandler,在函数里面监听的是"odometry/imu_incremental"（在下面imuPreintegration中发出，是imu 独立解算的里程计），把imu里程计放到imuodomQueue里面保存，然后把lidarOdomTime之前的数据扔掉，之后用最新和最旧时刻的mu里程计的计算差异，如图3所示，再加上lidarOdomAffine的基础进行发布。

另外还会发布一个path，用来rviz显示，名字叫lio-sam/imu/path。

/**
 * 激光雷达里程计就保存当前的雷达里程计的数据到lidarOdomAffine里面，
 * 把时间戳存到lidarOdomTime里面去，没干别的，
 * 具体的主要逻辑在imu 里程计的回调函数中处理。
 * 
 * */
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard lock(mtx);
 
        lidarOdomAffine = odom2affine(*odomMsg);
 
        lidarOdomTime = odomMsg->header.stamp.toSec();
    }
 
/**
 * 主要的处理逻辑在imu 里程计的回调函数中，`imuOdometryHandler`,
 * 在函数里面监听的是/imu/odometry_incremental（在下面imuPreintegration中发出，
 * 是imu 独立解算的里程计），把imu里程计放到imuodomQueue里面保存，
 * 然后把lidarOdomTime之前的数据扔掉，之后用最新和最旧时刻的mu里程计的计算差异，
 * 再加上lidarOdomAffine的基础进行发布。
 * */
    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        // static tf  这个地方究竟是在干什么？
        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_guard lock(mtx);
 
        imuOdomQueue.push_back(*odomMsg);
 
        // get latest odometry (at current IMU stamp)
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());//前
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// 后
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
        
        // publish latest odometry
        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);
 
        // publish tf
        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);
        // 发布 odom 到 baselink 的tf 关系
        tfOdom2BaseLink.sendTransform(odom_2_baselink);
 
        // publish IMU path
        static nav_msgs::Path imuPath;  // 静态数据，Path 实际是pose数组，
        static double last_path_time = -1;
        double imuTime = imuOdomQueue.back().header.stamp.toSec();
        // 控制发送频率 10HZ
        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);
            // lidar 时间戳1秒钟以内的数据，以前的数据删掉
            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);
            }
        }
    }

IMUPreintegration 类

成员变量列表：

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

1.1 imuhandle：

1.1.1 imu原始数据，会先被坐标系转换，通过调用头文件里的imuConverter函数，转换到一个“雷达中间系”中，其实和真正的雷达系差了一个平移。这里的操作就像对IMU 的数据进行修正，让IMU的坐标系与Lidar 坐标系的方向一致，然后 IMU 与 Lidar外参就只有位移关系了。乍一看，感觉作者故意写复杂了，但是个人猜想，1.由于IMU数据里面没有位移数据，不能进行直接变换到Lidar坐标系；2.速度变化很小，进行位移变换后是否影响积分精度。

1.1.2 转换后，会存入两个队列，一个imuQueOpt队列，在另一个回调函数odometryHandler会被处理，用于预积分因子源数据，用来优化系统状态（R，P，V，B）和imu的bias。一个imuQueImu队列，imuQueImu是这里真正要用的数据，每到一个imu数据，就用imuIntegratorImu_这个预积分器，把这一帧imu数据累积进去，然后预测当前时刻的状态：currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。

1.1.3 把currentState，通过imu2Lidar（其实是Lidar2Imu T_il）T_ol = T_oi * T_il，把IMU 积分得到的里程计转成真正的雷达里程计，然后发布出去。发布的话题就叫odometry/imu_incremental（实际是lidar 里程计），这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。虽然作者的变量命名很疑惑，但是代码实现得还是很老实的。

void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
    {
        std::lock_guard lock(mtx);
        // 这个函数这个地方写的非常绕，非值得注意的是，这里是把imu的数据转换到lidar坐标系下，
        // 但是又不完全转，只是保持与lidar坐标系的方向一致，差一个位移
        // 这里的操作就像对IMU 的数据进行修正，让IMU的坐标系与Lidar 坐标系的方向一致，
        // 然后 IMU 与 Lidar外参就只有位移关系了。
        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);  
        // 转换后的数据压入队列中
        imuQueOpt.push_back(thisImu);
        imuQueImu.push_back(thisImu);
        // 如果因子图还没有优化过，就直接返回了
        if (doneFirstOpt == false)
            return;
 
        // 获取该IMU数据的时间戳
        double imuTime = ROS_TIME(&thisImu);
        // 与上一帧IMU 数据时间戳作差，
        double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
        lastImuT_imu = imuTime;
 
        // 把当前帧的IMU 数据添加入预积分器
        // integrate this single imu message
        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);
 
        // 以上一帧lidar时刻因子图优化出来的状态为基准，预积分器预测出当前IMU时刻的位姿状态
        // predict odometry
        gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
 
        // publish odometry
        nav_msgs::Odometry odometry;
        odometry.header.stamp = thisImu.header.stamp;
        odometry.header.frame_id = odometryFrame;
        // 这里实际应该是“odom_lidar”
        odometry.child_frame_id = "odom_imu";
 
        // transform imu pose to ldiar
        gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
        // imu2Lidar 变量的值给得应该表示成T_il,整个式子 则为 T_ol = T_oi*T_il 
        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);
    }

1.2. odomHandle：这部分订阅的是/lio_sam/mapping/odometry_incremental，这个话题是由mapOptmization.cpp发布的，就是经过了地图匹配优化后的位姿信息，里程计就是从odom到lidar的变换关系而不是两帧激光之间的变换，它和imu里程计性质类似，就是相对世界坐标系的位姿。

1.2.1 初始化系统：

从imuQueOpt队列里，删掉当前这帧激光里程计时间上之前的数据，取两个Lidar时刻之间的IMU 数据进行预积分累积，然后把雷达的pose变换到IMU “坐标系”（这里的IMU坐标经过了修正，方向与Lidar坐标系保持一致，外参只差一个位移），然后初始化因子图优化器，相关优化变量prevPose等。因为预积分器需要bias ，而bias每次优化都会发生变化，所以需要重新设置，所以把两个预积分器imuIntegratorImu_，imuIntegratorOpt_给reset一下。（之后简称imu积分器和opt积分器）

1.2.2 清理缓存：

100帧激光里程计数据了，就把优化器给reset一下（用前面的先验协方差给reset一下）。每100 次 重新设置优化器，这个是gtsam 的实现要求的，在每次优化的过程中， gtsam 会缓存一些数据在optimizer贝叶斯树中，随着优化次数增大，该树会越来越大，执行效率就会变慢，所以要周期性清理。 在清理之前，需要把优化变量的协方差给保存起来，给新的优化器使用，在最开始的时候 每个因子的噪声模型都是人工经验值，这里需要把协方差矩阵传递下去，给新的优化器

1.2.3 正式处理：

1.2.3.1 把两个Lidar 时刻的IMU数据拿出来，送入opt积分器。这样得到两个时刻之间的IMU预积分，构建imu因子。

1.2.3.2 然后把关联Xkey-1 和Xkey的IMU 因子加入因子图 以及 激光里程计提供的pose先验因子，还有Bias BetweenFactor,整体做一个优化。优化的结果就是当前Lidar时刻的状态量（R，P，V，B）。

1.2.3.3 把优化的结果（主要是bias），重置opt积分器和imu积分器。由于imuIntegratorImu_被重置了BIAS ,需要对之前的IMU 数据进行重新积分，并删掉使用过的数据，这就是在imuHandler 中积分却不删数据的原因。

/**
 * @brief 激光雷达里程计的回调函数
 * 
 * @param odomMsg 
 */
    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard lock(mtx);
        // 获取lidar 时间戳
        double currentCorrectionTime = ROS_TIME(odomMsg);
 
        // make sure we have imu data to integrate
        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;
        // 是否退化，什么情况下会退化？ 初始情况下odomMsg->pose.covariance[0] == 1 
        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. initialize system
        if (systemInitialized == false)
        {
            // 重置Isam2 优化器
            积分
        // 优化器
        // optimizer = gtsam::ISAM2(optParameters);
        // 因子图容器
        // graphFactors = newGraphFactors;
        // 待优化的变量
        // graphValues = NewGraphValues;
            resetOptimization();
 
            // pop old IMU message
            while (!imuQueOpt.empty())
            {
                // imuQueOpt数据中小于lidar时刻的数据全部pop 出去
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }
            // 因子图是贝叶斯公式推导过来的，P(x|z) = a*P(z|x)P(x) 其中a 为系数，P(z|x)为似然函数，P(x) 则为先验
            // 所以 因子图里都要添加进 先验P(x)
            // 这里把 因子图中第一个优化变量设为先验
            // initial pose
            prevPose_ = lidarPose.compose(lidar2Imu);// compose 是什么运算？？转到IMU 系 todo:T_oi = T_ol * T_li
            gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise);
            graphFactors.add(priorPose);
            // initial velocity
            prevVel_ = gtsam::Vector3(0, 0, 0);
            gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise);
            graphFactors.add(priorVel);
            // initial bias
            prevBias_ = gtsam::imuBias::ConstantBias();
            gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise);
            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();
 
            //预积分器需要bias ，而bias每次优化都会发生变化，所以需要重新设置
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
            
            key = 1;
            systemInitialized = true;
            return;
        }
 
        // 每100 次 重新设置优化器，这个是gtsam 的实现要求的，在每次优化的过程中，
        // gtsam 会缓存一些数据在贝叶斯树中，随着优化次数增大，该树会越来越大，执行效率就会变慢，
        // 所以要周期性清理
        // reset graph for speed
        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 priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);
            // add velocity
            gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);
            // add bias
            gtsam::PriorFactor 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;
        }
 
        // 从这里开始 真正处理数据因子图优化了
        // 1. integrate imu data and optimize
        while (!imuQueOpt.empty())
        {
            // pop and integrate imu data that is between two optimizations
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            double imuTime = ROS_TIME(thisImu);
            // 取两个lidar 时间戳之间的IMU 数据提供给IMU积分器进行积分，
            if (imuTime < currentCorrectionTime - delta_t)
            {
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
                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;
                imuQueOpt.pop_front(); // 用一个 删一个，每一次优化周期内，没有上一周期的IMU数据
            }
            else
                break;
        }
        // add imu factor to graph
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
        // IMU 因子 提供 K-1 时刻的状态变量 与K 时刻的状态变量，与预积分器 
        // 在gtsam::ImuFactor 中定义好了 残差计算和雅可比的计算
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        graphFactors.add(imu_factor);
        // BIAS 残差 用gtsam::BetweenFactor 因子，表示两帧lidar时刻的BIAS 应该保持一致，对因子图进行约束
        // add imu bias between factor
        graphFactors.add(gtsam::BetweenFactor(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                         gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
        // lidar 位姿约束因子只跟当前 状态量有关系，属于一个先验因子
        // add pose factor
        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        graphFactors.add(pose_factor);
        // 因子相当于给因子图优化问题提供了观测值，
        // 还需要给优化变量提供初始值
        // 以上一时刻的状态为基准，从预积分器中预测当前的状态值，作为因子图优化变量的初值
        // insert predicted values
        gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);
        // optimize
        optimizer.update(graphFactors, graphValues);
        optimizer.update();
        // 优化完之后，因子图把相关约束 添加到优化器optimizer的贝叶斯树中，所以在因子图上可以删除
        graphFactors.resize(0);
        graphValues.clear();
        // Overwrite the beginning of the preintegration for the next step.
        gtsam::Values result = optimizer.calculateEstimate();
        prevPose_  = result.at(X(key));
        prevVel_   = result.at(V(key));
        prevState_ = gtsam::NavState(prevPose_, prevVel_);
        prevBias_  = result.at(B(key));
        // Reset the optimization preintegration object.
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        // check optimization
        if (failureDetection(prevVel_, prevBias_))
        {
            resetParams();
            return;
        }
 
        //因子图优化完之后，得到当前优化状态，需要设置imuIntegratorImu_ 的BIAS 重新预积分
 
        // 2. after optiization, re-propagate imu odometry preintegration
        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }
        // repropogate
        if (!imuQueImu.empty())
        {
            // reset bias use the newly optimized bias
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
            // integrate imu message from the beginning of this optimization
            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;
    }