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里程计信息
这里先简单梳理一下代码运行的流程(只看前三条语句), 实际上这三条是同时运行的, 这里是从数据的角度来说的. 理解为主嘛, 哈哈
-
先执行imu的回调函数imuHandler(),发现没有odometryHandler()的优化数据. 那就只把数据塞到两个队列里, 不做其他任何处理
-
接下来就进入了odometryHandler(),得到了一个lidar位姿T1. 先初始化系统, 丢掉比lidar位姿较早的imu数据(imuQueOpt里的), key=1(即此时为第一帧)
-
然后是执行发布imu_incrementa语句,但此时由于imuHandler()没做其他处理,所以也没有要发布的数据
-
于是再进入imuHandler(), 注意哦!! 此时仍没有odometryHandler()的优化数据.所以依旧只把数据塞到两个队列里, 不做其他任何处理
-
好,接下来是重点. 再进入odometryHandler(),得到lidar位姿T2 由于key=1帧我们已经完成初始化了. 所以接下来就要做优化了
-
首先要取出两个lidar位姿之间(即两帧之间)的imu数据, 由于我们在初始化时(即key=1帧)已经把T1之前的imu数据给pop出去了,所以只要把T2之前的imu数据取出来就是两帧之间的imu数据了(这里要仔细想一下). 并且我们要取一个就pop一个(这和我们在初始化阶段pop掉T1之前的数据是一回事),这是为了保证在下一帧能顺利取出T2和T3之间的imu数据
-
将两帧之间的imu数据送入预积分器(优化用的imuIntegratorOpt_ ,后面还有另一个), 并构建因子图约束, 这时的图模型只有T1,T2以及T1和T2之间的△imu预积分约束. 模型有了就可以做优化了,至于怎么优化的不用我们管, gtsam会帮我们做这件事. 把优化过的最佳估计值记录下来,后面要用. prevState_是优化过的T2(就是lidar的位姿和imu得到的速度,), prevBias是优化过的零偏. 这两个变量很重要, 我们接下来要用到
-
下一步就是把另一个队列imuQueImu中早与lidar位姿T2的imu数据pop出去, 并且用刚得到的优化过的零偏prevBiasOdom(就是prevBias)复位另一个预积分器imuIntegratorImu_ ,这个预积分器会在下一次进到imuHandler()发挥作用. 这时才把doneFirstOpt置成true( imuHandler()终于要开始搞事情了 )
-
-
pubImuOdometry此时还没有数据,所以还要等imuHandler()发挥作用
-
我们又一次来到了imuHandler(),这时odometryHandler()已经做过优化了,我们可以进行接下来的操作了. 把所有新来的imu数据加到imuIntegratorImu预积分器中, 此时应注意到这个预积分的起点是T2时刻,也就是说我们把T2后面的imu数据都加进来了. 然后我们就可以用前面优化好的prevStateOdom(实际上就是prevState)和prevBiasOdom进行预积分的状态估计了. 并且把估计好的odom数据记录下来,放到pubImuOdometry中
-
再执行pubImuOdometry语句, 把上面记录好的odom数据发布出去,这也就是我们发布的imu里程计了
事实上, 我们可以看到imu里程计在第二帧之后才发挥作用. 这么做的原因也很好理解,预积分算出来的只是一个△T位姿增量, 我们要结合一个较好的位姿初值T0, 才能得出良好的位姿估计结果,实时性和精确性都可以得到保障. 我们在第一帧只得到了一个位姿T1,所以没法进行优化. 在第二帧有了T2之后,我们也能取出T1和T2之间的imu构建一个预积分约束. 有了这三个节点,我们才能进行优化得到一个较好的位姿估计T0和imu零偏噪声, 在这二者的基础上,我们就可以做预积分发布imu里程计消息了.
imuHandler()
-
先将imu的数据做一个坐标转换,转到左前上坐标系下. 再把imu数据塞到两个队列中,一个进行imu位姿的优化,一个用来更新imu的最新状态
-
先判断在odometryHandler()是否对位姿进行了优化, 即有没有一个较好的初值. 有好的初值才能结合预积分发布较好的实时的imu里程计数据
-
如果odometryHandler()没做优化就继续塞数据,不做其他处理. 即在key=1之前imuHandler没做任何处理数据的事
-
odometryHandler()优化过后(即key=2),有了较好的里程计初值和偏差, 这样就能结合预积分发布可靠的imu里程计数据了
-
这里的imu_incremental实际上并非增量, 而是在odometryHandler()优化过的上一帧lidar位姿的基础上,结合最新的imu消息,进行预积分发布的里程计数据
-
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
-
{
-
std::lock_guard<std::mutex> lock(mtx);
-
// 首先把imu的数据做一个简单的转换
-
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
-
// 一个进行imu预积分和位姿的优化,一个用来更新imu的最新状态
-
imuQueOpt.push_back(thisImu);
-
imuQueImu.push_back(thisImu);
-
// 是否进行了优化,进行了优化才有新的prevBiasOdom
-
if (doneFirstOpt == false)
-
return;
-
-
double imuTime = ROS_TIME(&thisImu);
-
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
-
lastImuT_imu = imuTime;
-
-
// integrate this single imu message
-
// 每来一个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);
-
-
// predict odometry
-
// 根据这个值预测最新的状态
-
// 把预测的状态转到lidar坐标系下发布出去,保证实时性 此时的imu偏差是上一时刻的
-
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
-
-
// publish odometry
-
nav_msgs::Odometry odometry;
-
odometry.header.stamp = thisImu.header.stamp;
-
odometry.header.frame_id = odometryFrame;
-
odometry.child_frame_id = "odom_imu";
-
-
//ROS_INFO("key:%d, 1111id:%f", key, thisImu.header.stamp.toSec());
-
// transform imu pose to ldiar
-
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);
-
}
-
};
odometryHandler()
-
进入了odometryHandler(),得到第一个lidar位姿T1. 先初始化系统, 丢掉比lidar位姿较早的imu数据(imuQueOpt里的), key=1(即此时为第一帧)
-
首先要取出两个lidar位姿之间(即两帧之间)的imu数据, 由于我们在初始化时(即key=1帧)已经把T1之前的imu数据给pop出去了,所以只要把T2之前的imu数据取出来就是两帧之间的imu数据了(这里要仔细想一下). 并且我们要取一个就pop一个(这和我们在初始化阶段pop掉T1之前的数据是一回事),这是为了保证在下一帧能顺利取出T2和T3之间的imu数据
-
将两帧之间的imu数据送入预积分器(优化用的imuIntegratorOpt_ ,后面还有另一个), 并构建因子图约束, 这时的图模型只有T1,T2以及T1和T2之间的△imu预积分约束. 模型有了就可以做优化了,至于怎么优化的不用我们管, gtsam会帮我们做这件事. 把优化过的最佳估计值记录下来,后面要用. prevState_是优化过的T2(就是lidar的位姿和imu得到的速度,), prevBias是优化过的零偏. 这两个变量很重要, 我们接下来要用到
-
下一步就是把另一个队列imuQueImu中早与lidar位姿T2的imu数据pop出去, 并且用刚得到的优化过的零偏prevBiasOdom(就是prevBias)复位另一个预积分器imuIntegratorImu_ ,这个预积分器会在下一次进到imuHandler()发挥作用. 这时才把doneFirstOpt置成true
这篇好文章是转载于:学新通技术网
- 版权申明: 本站部分内容来自互联网,仅供学习及演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,请提供相关证据及您的身份证明,我们将在收到邮件后48小时内删除。
- 本站站名: 学新通技术网
- 本文地址: /boutique/detail/tanhgachee
-
photoshop保存的图片太大微信发不了怎么办
PHP中文网 06-15 -
Android 11 保存文件到外部存储,并分享文件
Luke 10-12 -
《学习通》视频自动暂停处理方法
HelloWorld317 07-05 -
word里面弄一个表格后上面的标题会跑到下面怎么办
PHP中文网 06-20 -
photoshop扩展功能面板显示灰色怎么办
PHP中文网 06-14 -
微信公众号没有声音提示怎么办
PHP中文网 03-31 -
excel下划线不显示怎么办
PHP中文网 06-23 -
怎样阻止微信小程序自动打开
PHP中文网 06-13 -
excel打印预览压线压字怎么办
PHP中文网 06-22 -
TikTok加速器哪个好免费的TK加速器推荐
TK小达人 10-01