loam源码地址： https://github.com/cuitaixiang/LOAM_NOTED.
论文学习： LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.
loam源码解析汇总：
loam源码解析1 ： scanRegistration（一）.
loam源码解析2 ： scanRegistration（二）.
loam源码解析3 ： laserOdometry（一）.
loam源码解析4 ： laserOdometry（二）.
loam源码解析5 ： laserOdometry（三）.
loam源码解析6 ： laserMapping（一）.
loam源码解析7 ： laserMapping（二）.
loam源码解析8 ： transformMaintenance.

一、概述

在阅读完loam的论文之后，开始对loam的开源框架进行学习。源码中主要包含scanRegistration、laserOdometry、laserMapping、transformMaintenance四个部分。

上图所示是loam的rosgraph，可以看出数据的主要流向是：

所以我们阅读源码的基本顺序也如上所述。本节主要是对scanRegistration.cpp的一些学习，主要实现的功能是对得到的点云和imu数据进行特征提取并发布出去，具体功能将逐一分析。

二、变量说明

关于雷达信息的一些配置：

//扫描周期, velodyne频率10Hz，周期0.1s
const double scanPeriod = 0.1;//初始化控制变量
const int systemDelay = 20;//弃用前20帧初始数据
int systemInitCount = 0;
bool systemInited = false;//激光雷达线数
const int N_SCANS = 16;

关于点云平滑度描述的一些变量：

//点云曲率, 40000为一帧点云中点的最大数量
float cloudCurvature[40000];
//曲率点对应的序号
int cloudSortInd[40000];
//点是否筛选过标志：0-未筛选过，1-筛选过
int cloudNeighborPicked[40000];
//点分类标号:2-代表曲率很大，1-代表曲率比较大,-1-代表曲率很小，0-曲率比较小(其中1包含了2,0包含了1,0和1构成了点云全部的点)
int cloudLabel[40000];

运动信息：

//imu时间戳大于当前点云时间戳的位置
int imuPointerFront = 0;
//imu最新收到的点在数组中的位置
int imuPointerLast = -1;
//imu循环队列长度
const int imuQueLength = 200;//点云数据开始第一个点的位移/速度/欧拉角
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
float imuVeloXStart = 0, imuVeloYStart = 0, imuVeloZStart = 0;
float imuShiftXStart = 0, imuShiftYStart = 0, imuShiftZStart = 0;//当前点的速度，位移信息
float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0;
float imuVeloXCur = 0, imuVeloYCur = 0, imuVeloZCur = 0;
float imuShiftXCur = 0, imuShiftYCur = 0, imuShiftZCur = 0;//每次点云数据当前点相对于开始第一个点的畸变位移，速度
float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0;
float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0;

IMU信息，包括时间、欧拉角、加速度、速度、位移：

double imuTime[imuQueLength] = {0};
float imuRoll[imuQueLength] = {0};
float imuPitch[imuQueLength] = {0};
float imuYaw[imuQueLength] = {0};float imuAccX[imuQueLength] = {0};
float imuAccY[imuQueLength] = {0};
float imuAccZ[imuQueLength] = {0};float imuVeloX[imuQueLength] = {0};
float imuVeloY[imuQueLength] = {0};
float imuVeloZ[imuQueLength] = {0};float imuShiftX[imuQueLength] = {0};
float imuShiftY[imuQueLength] = {0};
float imuShiftZ[imuQueLength] = {0};

ros发布消息：

ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubImuTrans;

三、主函数

int main(int argc, char** argv)
{ros::init(argc, argv, "scanRegistration");ros::NodeHandle nh;// 两个订阅话题：点云、IMU// 两个主要的回调函数：laserCloudHandler、imuHandlerros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_points", 2, laserCloudHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);// 6个发布话题pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);// ros::spin()进入循环，一直调用回调函数，用户输入Ctrl+C退出ros::spin();return 0;
}

四、IMU回调函数laserCloudHandler

1 接受IMU的角度和加速度信息

对于imu而言，需要消除重力加速度对测量的影响，设 g n = [ 0 , 0 , − 9.81 ] T g^n=[0,0,-9.81]^T gn=[0,0,−9.81]T,用欧拉角表示的旋转矩阵 R b n R_b^n Rbn​如下所示：

所以 R n b = R b n T R_n^b={R_b^n}^T Rnb​=Rbn​T,可以计算重力加速度引起的误差为： [ sin ⁡ θ ∗ 9.81 , − sin ⁡ Ψ ∗ sin ⁡ θ ∗ 9.81 , − cos ⁡ Ψ ∗ cos ⁡ θ ∗ 9.81 ] T [\sin\theta*9.81,-\sin\Psi*\sin\theta*9.81,-\cos\Psi*\cos\theta*9.81]^T [sinθ∗9.81,−sinΨ∗sinθ∗9.81,−cosΨ∗cosθ∗9.81]T,但是要注意imu的坐标系为x轴向前，y轴向左，z轴向上的右手坐标系。

//接收点云数据，velodyne雷达坐标系安装为x轴向前，y轴向左，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;//convert Quaternion msg to Quaterniontf::quaternionMsgToTF(imuIn->orientation, orientation);//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).//Here roll pitch yaw is in the global frametf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//循环移位效果，形成环形数组imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;AccumulateIMUShift();

2 AccumulateIMUShift积分速度与位移

在三维空间中，旋转矩阵分解到xyz轴：

以zxy定义旋转时，有 X i m u = R y R x R z X w X_{imu}=R_yR_xR_zX_w Ximu​=Ry​Rx​Rz​Xw​，所以 X w T = X i m u T R z − 1 R x − 1 R y − 1 {X_w}^T= {X_{imu}}^T{R_z}^{-1}{R_x}^{-1}{R_y}^{-1} Xw​T=Ximu​TRz​−1Rx​−1Ry​−1

void AccumulateIMUShift()
{float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)//绕z轴旋转(roll)float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//绕x轴旋转(pitch)float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;//绕y轴旋转(yaw)accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;

得到世界坐标系下的加速度之后,就可以计算世界坐标系下的位移和速度：
S l a s t = S b a c k + V b a c k ∗ T + a ∗ T 2 / 2 S_{last}=S_{back}+V_{back}*T+a*T^2/2 Slast​=Sback​+Vback​∗T+a∗T2/2
V l a s t = V b a c k ∗ T + a ∗ T V_{last}=V_{back}*T+a*T Vlast​=Vback​∗T+a∗T

  //上一个imu点int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;//上一个点到当前点所经历的时间，即计算imu测量周期double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义if (timeDiff < scanPeriod) {//（隐含从静止开始运动）//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;}
}了

五、畸变去除

主要是在收到IMU信息以后，通过IMU的信息对点云畸变处理，在laserCloudHandler（具体分析在下一章）被调用

1 位移畸变计算ShiftToStartIMU

计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变。
首先计算世界坐标系下的位移 δ S w = S c u r − S s t a r t − V s t a r t ∗ T \delta S_w=S_{cur}-S_{start}-V_{start}*T δSw​=Scur​−Sstart​−Vstart​∗T，
再转换到局部坐标系下 δ S s c a n = R z − 1 R x − 1 R y − 1 δ S w \delta S_{scan}=R_z^{-1}R_x^{-1}R_y^{-1}\delta S_w δSscan​=Rz−1​Rx−1​Ry−1​δSw​：

void ShiftToStartIMU(float pointTime)
{//计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)//imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tgtransfrom from the global frame to the local frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuShiftFromStartZCur = z2;
}

2 速度畸变计算VeloToStartIMU

计算局部坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变（增量）,
首先计算世界坐标系下的位移 δ V w = V c u r − V s t a r t \delta V_w=V_{cur}-V_{start} δVw​=Vcur​−Vstart​，
再转换到局部坐标系下 δ v s c a n = R z − 1 R x − 1 R y − 1 δ V w \delta v_{scan}=R_z^{-1}R_x^{-1}R_y^{-1}\delta V_w δvscan​=Rz−1​Rx−1​Ry−1​δVw​：

void VeloToStartIMU()
{//计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vgtransfrom from the global frame to the local frame*********************************************************************************///绕y轴旋转(-imuYawStart)，即Ry(yaw).inversefloat x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inversefloat x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverseimuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuVeloFromStartZCur = z2;
}

3 去除畸变TransformToStartIMU

将点云坐标通过imu的旋转关系转移到世界坐标系下（ X i m u − > X w X_{imu}->X_w Ximu​−>Xw​），再通过当前帧开始时的角度，从世界坐标系下转移到当前帧的初始坐标系下( X w − > X s c a n X_w->X_{scan} Xw​−>Xscan​)。最后再加上ShiftToStartIM中计算出来的位移误差即完成补偿。

//去除点云加减速产生的位移畸变
void TransformToStartIMU(PointType *p)
{/********************************************************************************Ry*Rx*Rz*Pl, transform point to the global frame*********************************************************************************///绕z轴旋转(imuRollCur)float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;//绕x轴旋转(imuPitchCur)float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;//绕y轴旋转(imuYawCur)float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pgtransfrom global points to the local frame*********************************************************************************///绕y轴旋转(-imuYawStart)float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;float y4 = y3;float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;//绕x轴旋转(-imuPitchStart)float x5 = x4;float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;//绕z轴旋转(-imuRollStart)，然后叠加平移量p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;
}

loam源码解析1 ： scanRegistration（一）相关推荐

1. loam源码解析5 ： laserOdometry（三）

   transformMaintenance.cpp解析 八.位姿估计 1. 雅可比计算 2. 矩阵求解 3. 退化问题分析 4. 姿态更新 5. 坐标转换 loam源码地址: https://githu ...

2. LOAM源码解析1一scanRegistration

   鉴于工作和学习需要,学习了激光salm算法loam,并阅读了作者的原版论文,现将学习过程中的理解与一些源码剖析记录整理下来,也是对于学习slam的阶段性总结!!! 一.综述 LOAM这篇论文是发表于2 ...

3. LOAM源码解析2——laserOdometry

   这是LOAM第二部分Lidar laserOdometry雷达里程计. 在第一章提取完特征点后,需要对特征点云进行关联匹配,之后估计姿态. 主要分为两部分: 特征点关联使用scan-to-scan方式 ...

4. LOAM源码结合论文解析（二）laserOdometry

   中文注释版本.本文大多注释来自 --- https://github.com/daobilige-su/loam_velodyne laserOdometry节点接收scanResgistraion传 ...

5. A-LOAM源码解析

   声明:本文是笔者在学习SLAM时发现的好文,与诸君分享 转载自:https://www.cnblogs.com/wellp/p/8877990.html 导读 下面是我对LOAM论文的理解以及对A-L ...

6. Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）

   Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一) 1. LiDAR inertial odometry and mapping简介 ...

7. 谷歌BERT预训练源码解析（二）：模型构建

   版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...

8. 谷歌BERT预训练源码解析（三）：训练过程

   目录 前言 源码解析 主函数 自定义模型 遮蔽词预测 下一句预测 规范化数据集 前言 本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...

9. 谷歌BERT预训练源码解析（一）：训练数据生成

   目录 预训练源码结构简介 输入输出 源码解析 参数 主函数 创建训练实例 下一句预测&实例生成 随机遮蔽 输出 结果一览 预训练源码结构简介 关于BERT,简单来说,它是一个基于Transfo ...

最新文章

1. 计算机系统安装和维护实验,2计算机系统安装维护实验报告.doc
2. ASP.NET自定义控件组件开发 第四章 组合控件开发CompositeControl
3. android 学习笔记（八）building system8.5 shell脚本的学习
4. Vue_注册登录（短信验证码登录）
5. Objective-C设计模式（MVC）的实现,以及协议与委托的运用
6. web软件测试 测试报告模板_杭州软件测试培训要多长时间？需要学习什么内容？...
7. 设计算法时要确保分类讨论的完备性
8. Java 整数型的进制间的互相转换
9. requests源码分析
10. oracle数据库11gr2,Oracle 11g R2 X64数据库安装
11. 【前端工程师手册】说清楚JavaScript中的相等性判断
12. php-php连接数据库
13. （43）System Verilog 类中变量随机化
14. 小数变百分数_小数除法三要“点”
15. 使用ActiveReports for .net 进行报表开发（十）--交叉变换背景 (转)
16. java编程的逻辑 京东,从阿里，京东等大厂面试题中提炼出25道最频繁出现的并发编程难题（附答案）...
17. CSS元素隐藏原理和效果小结
18. Codeforces Round #573 (Div. 2) C. Tokitsukaze and Discard Items
19. Unity3D研究院之解决ttf繁体字体不显示问题
20. 谷歌浏览器导出导入插件

热门文章

1. Cocos Creator 3.x 动态加载 龙骨动画
2. linux如何查看zst文件,Linux下如何查看共享文件夹
3. 兔子感悟：春运火车站十种死法
4. 在 macOS 上安装 AWS CLI
5. 计算机毕业设计ssm康复系统4y703系统+程序+源码+lw+远程部署
6. 金桥太茂商业广场——好奇5界 黄金周庆典
7. 用四阶龙格-库塔方法求微分方程组
8. (Pycharm新版专业版)初次部署无法同步文件，报错信息：找不到要处理的文件或文件夹
9. 规划网站的目录结构时通常遵循一定的规则
10. Java Virtual Machine（一）