lio-sam运行思路——预积分模块imuHandler()

imu预积分模块

预积分模块订阅了两个话题imuTopic和lio_sam/mapping/odometry_incremental,发布了一个话题odometry/imu_incremental

• imuTopic实际上是imu/data_raw ,即imu的原始数据. imu_correct是作者修改驱动后发布的数据格式. 该回调函数的作用就是将获取的imu数据送到两个队列中,分别用于优化和预测. 并发布imu对轨迹的预测imu_incremental

• odometry_incremental*订阅了imu的增量数据imu_incremental利用该增量数据, 并结合之前接收的上一时刻的激光里程计的位姿信息, 解算当前时刻的imu里程计信息

这里先简单梳理一下代码运行的流程(只看前三条语句), 实际上这三条是同时运行的, 这里是从数据的角度来说的. 理解为主嘛, 哈哈

1. 先执行imu的回调函数imuHandler(),发现没有odometryHandler()的优化数据. 那就只把数据塞到两个队列里, 不做其他任何处理

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

3. 然后是执行发布imu_incrementa语句,但此时由于imuHandler()没做其他处理,所以也没有要发布的数据

4. 于是再进入imuHandler(), 注意哦!! 此时仍没有odometryHandler()的优化数据.所以依旧只把数据塞到两个队列里, 不做其他任何处理

5. 好,接下来是重点. 再进入odometryHandler(),得到lidar位姿T2 由于key=1帧我们已经完成初始化了. 所以接下来就要做优化了

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

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

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

6. pubImuOdometry此时还没有数据,所以还要等imuHandler()发挥作用

7. 我们又一次来到了imuHandler(),这时odometryHandler()已经做过优化了,我们可以进行接下来的操作了. 把所有新来的imu数据加到imuIntegratorImu预积分器中, 此时应注意到这个预积分的起点是T2时刻,也就是说我们把T2后面的imu数据都加进来了. 然后我们就可以用前面优化好的prevStateOdom(实际上就是prevState)和prevBiasOdom进行预积分的状态估计了. 并且把估计好的odom数据记录下来,放到pubImuOdometry中

8. 再执行pubImuOdometry语句, 把上面记录好的odom数据发布出去,这也就是我们发布的imu里程计了

事实上, 我们可以看到imu里程计在第二帧之后才发挥作用. 这么做的原因也很好理解,预积分算出来的只是一个△T位姿增量, 我们要结合一个较好的位姿初值T0, 才能得出良好的位姿估计结果,实时性和精确性都可以得到保障. 我们在第一帧只得到了一个位姿T1,所以没法进行优化. 在第二帧有了T2之后,我们也能取出T1和T2之间的imu构建一个预积分约束. 有了这三个节点,我们才能进行优化得到一个较好的位姿估计T0和imu零偏噪声, 在这二者的基础上,我们就可以做预积分发布imu里程计消息了.

imuHandler()

1. 先将imu的数据做一个坐标转换,转到左前上坐标系下. 再把imu数据塞到两个队列中,一个进行imu位姿的优化,一个用来更新imu的最新状态

2. 先判断在odometryHandler()是否对位姿进行了优化, 即有没有一个较好的初值. 有好的初值才能结合预积分发布较好的实时的imu里程计数据

3. 如果odometryHandler()没做优化就继续塞数据,不做其他处理. 即在key=1之前imuHandler没做任何处理数据的事

4. odometryHandler()优化过后(即key=2),有了较好的里程计初值和偏差, 这样就能结合预积分发布可靠的imu里程计数据了

5. 这里的imu_incremental实际上并非增量, 而是在odometryHandler()优化过的上一帧lidar位姿的基础上,结合最新的imu消息,进行预积分发布的里程计数据

1.  
   void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
2.  
   {
3.  
   std::lock_guard<std::mutex> lock(mtx);
4.  
   // 首先把imu的数据做一个简单的转换
5.  
   sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
6.  
   // 一个进行imu预积分和位姿的优化,一个用来更新imu的最新状态
7.  
   imuQueOpt.push_back(thisImu);
8.  
   imuQueImu.push_back(thisImu);
9.  
   // 是否进行了优化,进行了优化才有新的prevBiasOdom
10.  
    if (doneFirstOpt == false)
11.  
    return;
12.  
     
13.  
    double imuTime = ROS_TIME(&thisImu);
14.  
    double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
15.  
    lastImuT_imu = imuTime;
16.  
     
17.  
    // integrate this single imu message
18.  
    // 每来一个imu数据加入到预积分器中
19.  
    imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
20.  
    gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
21.  
     
22.  
    // predict odometry
23.  
    // 根据这个值预测最新的状态
24.  
    // 把预测的状态转到lidar坐标系下发布出去,保证实时性 此时的imu偏差是上一时刻的
25.  
    gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
26.  
     
27.  
    // publish odometry
28.  
    nav_msgs::Odometry odometry;
29.  
    odometry.header.stamp = thisImu.header.stamp;
30.  
    odometry.header.frame_id = odometryFrame;
31.  
    odometry.child_frame_id = "odom_imu";
32.  
     
33.  
    //ROS_INFO("key:%d, 1111id:%f", key, thisImu.header.stamp.toSec());
34.  
    // transform imu pose to ldiar
35.  
    gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
36.  
    gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
37.  
     
38.  
    odometry.pose.pose.position.x = lidarPose.translation().x();
39.  
    odometry.pose.pose.position.y = lidarPose.translation().y();
40.  
    odometry.pose.pose.position.z = lidarPose.translation().z();
41.  
    odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
42.  
    odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
43.  
    odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
44.  
    odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
45.  
     
46.  
    odometry.twist.twist.linear.x = currentState.velocity().x();
47.  
    odometry.twist.twist.linear.y = currentState.velocity().y();
48.  
    odometry.twist.twist.linear.z = currentState.velocity().z();
49.  
    odometry.twist.twist.angular.x = thisImu.angular_velocity.x prevBiasOdom.gyroscope().x();
50.  
    odometry.twist.twist.angular.y = thisImu.angular_velocity.y prevBiasOdom.gyroscope().y();
51.  
    odometry.twist.twist.angular.z = thisImu.angular_velocity.z prevBiasOdom.gyroscope().z();
52.  
    pubImuOdometry.publish(odometry);
53.  
    }
54.  
    };

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

这篇好文章是转载于：学新通技术网