3d激光SLAM：LIO-SAM框架—IMU预积分功能数据初始化

最新推荐文章于 2024-05-21 12:56:38 发布

3Ｄ视觉工坊

最新推荐文章于 2024-05-21 12:56:38 发布

阅读量1.1k

点赞数

文章标签： 3d 机器人

原文链接：https://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247632325&idx=4&sn=76b3802d96465772345d02778d965981&chksm=fbf2eef1cc8567e7455d830237e2960efef3054a965b0ed41c1d5036f7e171650af9069bf5bc&scene=126&&sessionid=0

版权

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。

实现了高精度、实时的移动机器人的轨迹估计和建图。

本篇博客重点解读LIO-SAM框架下IMU预积分功能数据初始化代码部分

LIO-SAM 的代码主要在其主目录内的src文件夹下的四个cpp文件，分别是：

featureExtraction.cpp
imageProjection.cpp
imuPreintegration.cpp
mapOptmization.cpp

每个cpp文件是一个独立的ROS节点，对应着下图的四个模块

lio-sam的所有文件即对应功能在下面做了如下总结：

lio-sam:.│  CMakeLists.txt   #项目工程配置文件，可以知道作者用了哪些第三方库及cpp生成了什么执行文件│  LICENSE          #软件版权│  package.xml      #ROS包配置文件│  README.md        #项目工程说明文件：文件构成、依赖、运行等│  ├─config│  │  params.yaml  #参数文件│  │  │  └─doc   #存储效果图、流程图、论文等│      │  │      └─kitti2bag  #将kitti数据集转换成bag格式│              kitti2bag.py│              README.md│              ├─include│      utility.h  #参数服务器类，初始化参数；各类公用函数│      ├─launch│  │  run.launch #总运行launch文件│  │  │  └─include  #分模块运行文件│      │  module_loam.launch│      │  module_navsat.launch│      │  module_robot_state_publisher.launch│      │  module_rviz.launch│      │  │      ├─config #存储rviz参数文件和机器人坐标系参数│      │      rviz.rviz│      │      robot.urdf.xacro│              ├─msg│      cloud_info.msg #自定义ROS数据格式│      ├─src #源文件│      featureExtraction.cpp #提取雷达线面特征，发布雷达点云│      imageProjection.cpp   #订阅提取的雷达点云、IMU数据和IMU里程计数据，对雷达做畸变矫正，进行雷达前端里程计位姿粗估计的发布（以IMU频率）│      imuPreintegration.cpp #IMU预积分，订阅雷达里程计和IMU数据，估计IMU偏置，进行雷达里程计、IMU预积分因子的图优化，输出IMU里程计。│      mapOptmization.cpp    #订阅雷达前端信息、GPS信息，进行点云配准，进行雷达里程计、全局GPS、回环检测因子的图优化。│      └─srv        save_map.srv

本篇主要解读 IMU预积分部分代码，也就是

imuPreintegration.cpp #IMU预积分，订阅雷达里程计和IMU数据，估计IMU偏置，进行雷达里程计、IMU预积分因子的图优化，输出IMU里程计。

代码解读

int main(int argc, char** argv){    ros::init(argc, argv, "roboat_loam");    IMUPreintegration ImuP;//IMUPreintegration 类的实例    TransformFusion TF;//TransformFusion 类的实例    ROS_INFO("\033[1;32m----&gt; IMU Preintegration Started.\033[0m");//打印消息    ros::MultiThreadedSpinner spinner(4);//开四个线程 通过并发的方式使得速度得到提升    spinner.spin();//程序执行到这个地方 则等待 topic 回调函数执行    return 0;}

main函数部分很简洁，功能主要完成部分都在定义的两个类中进行。

在main函数中进行

节点初始化
IMUPreintegration 类的实例
TransformFusion 类的实例
打印消息
开四个线程通过并发的方式使得速度得到提升
等待 topic 回调函数执行
之后则看 IMUPreintegration 这个类，先看构造函数部分

在里面首先进行了订阅imu信息

subImu      = nh.subscribe&lt;sensor_msgs::Imu&gt;  (imuTopic,2000, &amp;IMUPreintegration::imuHandler,this, ros::TransportHints().tcpNoDelay());

imuTopic 为 imu_correct, imu原始数据，这个imuTopic是从参数服务器读取的，具体的配置在prams.yaml中

如果你的imu的topic和默认的不一致则需要修改

然后可以看其具体的回调函数 imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr&amp; imu_raw)    {        std::lock_guard&lt;std::mutex&gt; lock(mtx);        //首先把imu的状态做一个简单的转换        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);        // 注意这里有两个imu队列，作用不相同，一个用来执行预积分和位姿变换的优化，一个用来更新最新imu状态          imuQueOpt.push_back(thisImu);        imuQueImu.push_back(thisImu);        // 如果没有发生过优化 则 return        if (doneFirstOpt == false)            return;

回调函数先把imu的状态做一个简单的转换，转到lidar坐标系下

将转换后的imu数据存入两个队列中，注意这里有两个imu队列，作用不相同，一个用来执行预积分和位姿变换的优化，一个用来更新最新imu状态

如果没有发生过优化则 retur，doneFirstOpt这个标志位，在接受到帧间里程计信息后，则至为true，imuConverter函数在utility.h文件中

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu&amp; imu_in)    {        sensor_msgs::Imu imu_out = imu_in;        // rotate acceleration  //进行加速度坐标旋转        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);        acc = extRot * acc;        imu_out.linear_acceleration.x = acc.x();        imu_out.linear_acceleration.y = acc.y();        imu_out.linear_acceleration.z = acc.z();        // rotate gyroscope  // 进行陀螺仪坐标旋转        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);        gyr = extRot * gyr;        imu_out.angular_velocity.x = gyr.x();        imu_out.angular_velocity.y = gyr.y();        imu_out.angular_velocity.z = gyr.z();        // rotate roll pitch yaw // 进行姿态角坐标旋转        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);        Eigen::Quaterniond q_final = q_from * extQRPY;        imu_out.orientation.x = q_final.x();        imu_out.orientation.y = q_final.y();        imu_out.orientation.z = q_final.z();        imu_out.orientation.w = q_final.w();        //检测姿态数据是否正常        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) &lt; 0.1)        {            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");            ros::shutdown();        }        return imu_out;//返回变换后的imu数据    }};

进行加速度坐标旋转、进行陀螺仪坐标旋转、进行姿态角坐标旋转、检测姿态数据是否正常、返回变换后的imu数据

在进行加速度和陀螺仪变换的时候，使用的是extRot，该参数的根源来源于prams.yaml中

进行姿态角坐标旋转，使用的是extQRPY，该参数的根源来源于prams.yaml中

所有终于明白为什么在配置文件中有两个外参了！

imuHandler这个回调函数，先看到这部分，后面的之后再看，需要回到上面的IMUPreintegration的构造函数，看订阅到帧间里程计信息做了哪些事情。

subOdometry = nh.subscribe&lt;nav_msgs::Odometry&gt;("lio_sam/mapping/odometry_incremental", 5,    &amp;IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

订阅雷达里程计信息，lio_sam/mapping/odometry_incremental 是mapOptmization发出的，odometryHandler回调函数，走起

double currentCorrectionTime = ROS_TIME(odomMsg);

通过ROS_TIME函数把消息中的时间戳取了出来

if (imuQueOpt.empty())            return;

保证imu队列中有数据

float p_x = odomMsg-&gt;pose.pose.position.x;        float p_y = odomMsg-&gt;pose.pose.position.y;        float p_z = odomMsg-&gt;pose.pose.position.z;        float r_x = odomMsg-&gt;pose.pose.orientation.x;        float r_y = odomMsg-&gt;pose.pose.orientation.y;        float r_z = odomMsg-&gt;pose.pose.orientation.z;        float r_w = odomMsg-&gt;pose.pose.orientation.w;

通过里程计话题获得位置信息四元数获得雷达里程计位姿

bool degenerate = (int)odomMsg-&gt;pose.covariance[0] == 1 ? true : false;

该位姿是否出现退化 pose.covariance[0] 为1 则雷达里程计有退化风险，该帧位姿精度有一定程序下降

gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

把位姿转成 gtsam的格式，进入系统的初始化，下面部分仅执行一次

resetOptimization();

在函数内部初始化gtsam的一些量

while (!imuQueOpt.empty())            {                if (ROS_TIME(&amp;imuQueOpt.front()) &lt; currentCorrectionTime - delta_t)                {                    lastImuT_opt = ROS_TIME(&amp;imuQueOpt.front());                    imuQueOpt.pop_front();                }                else                    break;            }

将这个雷达里程计之前的imu信息全部扔掉，整个LIO-SAM中作者对时间同步这块的思想都是这样的，保证imu与odometry消息时间同步因为imu是高频数据所以这是必要的

prevPose_ = lidarPose.compose(lidar2Imu);

将lidar的位姿移到imu坐标系下，lidar2Imu 是lidar到imu的外参，compose是gtsam的一个功能函数，VIO和LIO的框架都在在IMU坐标系下进行的

gtsam::PriorFactor&lt;gtsam::Pose3&gt; priorPose(X(0), prevPose_, priorPoseNoise);            graphFactors.add(priorPose);

设置其初始位姿和置信度，约束加入到因子中

gtsam::PriorFactor 模块涉及到的变量结点
gtsam::Pose3 表示六自由度位姿
gtsam::Vector3 表示三自由度速度
gtsam::imuBias::ConstantBias 表示IMU零偏

以上也是预积分模型中涉及到的三种状态变量

gtsam::PriorFactor 为先验因子，表示对某个状态量T的一个先验估计，约束某个状态变量的状态不会离该先验值过远。

其中的X(0)的，初始定义如下。事先的符号

priorPoseNoise 是先验位姿的噪声，该值为

priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) &lt;&lt; 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m

初始位姿置信度设置比较高后面构成协方差矩阵值越小表示置信度越高

prevVel_ = gtsam::Vector3(0, 0, 0);            gtsam::PriorFactor&lt;gtsam::Vector3&gt; priorVel(V(0), prevVel_, priorVelNoise);               graphFactors.add(priorVel);

和上面位姿基本一样初始化速度，这里直接赋 0 了，将速度约束加到因子图中，其中priorVelNoise 速度的噪声是

priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s

初始化速度置信度设置差些因为速度一开始设置的是0，不知道是多少

prevBias_ = gtsam::imuBias::ConstantBias();            gtsam::PriorFactor&lt;gtsam::imuBias::ConstantBias&gt; priorBias(B(0), prevBias_, priorBiasNoise);                  graphFactors.add(priorBias);

初始化IMU 零偏，将零偏约束加到因子图中，gtsam::imuBias::ConstantBias()是gtsam做好的一个imu零偏，其中都是0，所以对应bias的噪声置信度也要设置的高些

priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good

以上把约束加入完毕，下面就开始添加状态量

graphValues.insert(X(0), prevPose_);            graphValues.insert(V(0), prevVel_);            graphValues.insert(B(0), prevBias_);

给各个状态量赋成初始值

optimizer.update(graphFactors, graphValues);

约束和状态量更新进isam优化器

graphFactors.resize(0);            graphValues.clear();

进优化器之后保存约束和状态量的变量就清零

imuIntegratorImu_-&gt;resetIntegrationAndSetBias(prevBias_);            imuIntegratorOpt_-&gt;resetIntegrationAndSetBias(prevBias_);

预积分的接口，使用初始零偏进行初始化之前imu有两个队列，每个队列对应预积分处理器

key = 1;
            systemInitialized = true;//系统初始化完成
            return;

系统初始化完成

总结

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
 3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
 6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
 7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~

3Ｄ视觉工坊

关注

0
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
3d激光SLAM：LIO-SAM框架—IMU预积分功能数据初始化

前言LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里...
复制链接

扫一扫