lio-sam运行思路整理——预积分模块odometryHandler()

1. 进入了odometryHandler(),得到第一个lidar位姿T1. 先初始化系统, 丢掉比lidar位姿较早的imu数据(imuQueOpt里的), key=1(即此时为第一帧)

2. 首先要取出两个lidar位姿之间(即两帧之间)的imu数据, 由于我们在初始化时(即key=1帧)已经把T1之前的imu数据给pop出去了,所以只要把T2之前的imu数据取出来就是两帧之间的imu数据了(这里要仔细想一下). 并且我们要取一个就pop一个(这和我们在初始化阶段pop掉T1之前的数据是一回事),这是为了保证在下一帧能顺利取出T2和T3之间的imu数据

3. 将两帧之间的imu数据送入预积分器(优化用的imuIntegratorOpt_ ,后面还有另一个), 并构建因子图约束, 这时的图模型只有T1,T2以及T1和T2之间的△imu预积分约束. 模型有了就可以做优化了,至于怎么优化的不用我们管, gtsam会帮我们做这件事. 把优化过的最佳估计值记录下来,后面要用. prevState_是优化过的T2(就是lidar的位姿和imu得到的速度,), prevBias是优化过的零偏. 这两个变量很重要, 我们接下来要用到

4. 下一步就是把另一个队列imuQueImu中早与lidar位姿T2的imu数据pop出去, 并且用刚得到的优化过的零偏prevBiasOdom(就是prevBias)复位另一个预积分器imuIntegratorImu_ ,这个预积分器会在下一次进到imuHandler()发挥作用. 这时才把doneFirstOpt置成true

void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);

        double currentCorrectionTime = ROS_TIME(odomMsg);

        // make sure we have imu data to integrate
        // 确保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::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)
        {
            // 优化问题复位
            resetOptimization();

            // pop old IMU message
            // 将之前的imu消息丢掉
            while (!imuQueOpt.empty())
            {
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }