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的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。
本篇博客重点解读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----> IMU Preintegration Started.\033[0m");//打印消息 ros::MultiThreadedSpinner spinner(4);//开四个线程 通过并发的方式使得速度得到提升 spinner.spin();//程序执行到这个地方 则等待 topic 回调函数执行 return 0;}
main函数部分很简洁,功能主要完成部分都在定义的两个类中进行。
在main函数中进行
节点初始化
IMUPreintegration 类的实例
TransformFusion 类的实例
打印消息
开四个线程 通过并发的方式使得速度得到提升
等待 topic 回调函数执行
之后则看 IMUPreintegration 这个类,先看构造函数部分
在里面首先进行了 订阅imu信息
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic,2000, &IMUPreintegration::imuHandler,this, ros::TransportHints().tcpNoDelay());
imuTopic 为 imu_correct, imu原始数据,这个imuTopic是从参数服务器读取的,具体的配置在prams.yaml中
如果你的imu的topic和默认的不一致则需要修改
然后可以看其具体的回调函数 imuHandler
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); // 如果没有发生过优化 则 return if (doneFirstOpt == false) return;
回调函数先把imu的状态做一个简单的转换,转到lidar坐标系 下
将转换后的imu数据存入两个队列中,注意这里有两个imu队列,作用不相同,一个用来执行预积分和位姿变换的优化,一个用来更新最新imu状态
如果没有发生过优化 则 retur,doneFirstOpt这个标志位,在接受到帧间里程计信息后,则至为true,imuConverter函数在utility.h文件中
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& 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()) < 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<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &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->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;
该位姿是否出现退化 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(&imuQueOpt.front()) < currentCorrectionTime - delta_t) { lastImuT_opt = ROS_TIME(&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<gtsam::Pose3> 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) << 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<gtsam::Vector3> 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<gtsam::imuBias::ConstantBias> 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_->resetIntegrationAndSetBias(prevBias_); imuIntegratorOpt_->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重建系统[理论+源码+实践]
9.单目深度估计方法:算法梳理与代码实现
10.自动驾驶中的深度学习模型部署实战
11.相机模型与标定(单目+双目+鱼眼)
12.重磅!四旋翼飞行器:算法与实战
13.ROS2从入门到精通:理论与实战
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天内无条件退款
高质量教程资料、答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~
3d激光SLAM:LIO-SAM框架—IMU预积分功能数据初始化相关推荐
- 3d激光SLAM:LIO-SAM框架---位姿融合输出
3d激光SLAM:LIO-SAM框架---位姿融合输出 前言 Eigen::Affine3f 位姿融合输出 result 前言 LIO-SAM的全称是:Tightly-coupled Lidar In ...
- 3d激光slam:LIO-SAM框架---特征点提取
3d激光slam:LIO-SAM框架---特征点提取 前言 特征点提取 Result 前言 LIO-SAM的全称是:Tightly-coupled Lidar Inertial Odometry vi ...
- 十七.激光和惯导LIO-SLAM框架学习之IMU和IMU预积分
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
- 彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总!
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...
- 3D激光雷达SLAM算法学习01——3D激光SLAM整体框架
#1.3D激光雷达系列主要内容 结合个人毕业设计和日后发展方向,开始学习3D激光雷达的相关感知算法,预计SLAM(定位和建图)开始,本篇给出简单的框架和绪论,后边会按照图优化/滤波原理.传感器基础.数 ...
- SLAM学习笔记(十九)开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
- 3D激光SLAM:LOAM 论文--算法详细解读
3D激光SLAM:LOAM 论文--算法详细解读 LOAM简介 论文里面的符号表示 算法部分 激光雷达里程计 A 特征点提取 B 找特征点的匹配对 C 运动估计 lidar 建图 测试结果 LOAM是 ...
- 32线镭神雷达跑LeGO-LOAM:3D 激光SLAM
32线镭神雷达跑LeGO-LOAM:3D 激光SLAM 安装LeGO-LOAM 镭神雷达的相关修改 LeGO-LOAM的修改 修改utility.h 修改imageproject.cpp Enjoy ...
- 3D激光SLAM:Livox激光雷达硬件时间同步
3D激光SLAM:Livox激光雷达硬件时间同步 前言 同步原理 PTP时间同步原理 GPS时间同步原理 PPS时间同步原理 GPS+PPS时间同步使用方法 Livox Hub Livox Conve ...
最新文章
- 初识FPGA(二)(FPGA与ASIC及CPLD的对比)
- 秒懂 QPS、TPS、PV、UV、GMV、IP、RPS!
- 社会化分享插件集成分享
- linux的du使用方法
- C#通过修改注册表改变IE默认选项
- ossim系统下nagios监控机器可用性用rrd图形显示
- 苹果大中华区营收锐减 决定去印度市场“掘金”
- NUMA - Non Uniform Memory Architecture 非统一内存架构
- Android Intent组件使用
- 数据库优化常用的途径(方法)
- 书生中学计算机应用自费,浙江省台州市书生中学2016-2017学年高二上学期期中考试信息试题 Word版含答案.doc...
- 基于AT89S52的俄罗斯方块游戏设计与实现
- C语言文件操作(含详细步骤)
- jquery设置ajax全局参数
- scrapy爬取51job职位信息(针对2020.851job新的反爬虫机制)
- 无限城app为什么服务器繁忙,鬼灭之刃:无惨为什么敢一人前往无限城,其实他最大底牌就是上五...
- 人工智能研究和应用领域
- STM32蓝牙控制循迹避障小车源代码——3.舵机、超声波测距模块
- 关于JS的20多个小技巧
- 拜日式精准引导词_瑜伽拜日式全套引导词