lego-loam代码分析（3）-激光里程计
lego-loam代码分析（3）-激光里程计
- 匹配初始化
- TransformToStart
- TransformToEnd
- 两次LM匹配
- 平面匹配
- 匹配点查找
- 目标点到匹配平面的距离
- LM求解
- 角点匹配
- 匹配点查找
- 目标点到直线的距离
- odometry 发布
- 发布建图数据
上节分析了其点云特征提取，目的是用于帧间匹配获取激光里程计。而像常见的点云匹配如NDT、ICP等匹配算法，均是对整个点云进行匹配。而loam为提高效率和实时性，则采用特征点进行匹配。
匹配初始化
由于激光里程计相临两帧点云进行匹配获取，即当前帧与上一帧进行匹配，故开启slam收到的第一帧点云数据则需要对cloud_last进行赋值，即对上一帧进行记录和存储，不做匹配运算。
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp; // 第一次last 和 curr 应一样， 即赋初始状态cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;kdtreeCornerLast.setInputCloud(laserCloudCornerLast); // 初始化KD TREEkdtreeSurfLast.setInputCloud(laserCloudSurfLast);laserCloudCornerLastNum = laserCloudCornerLast->points.size(); // 获取两种特征点云个数laserCloudSurfLastNum = laserCloudSurfLast->points.size();
TransformToStart
激光点云进行运动补偿很容易理解，由于雷达扫描一圈是需要时间的，如果激光雷达静止不动，则一圈所有激光光束测量的距离为基于同一个原点测量所得；而若雷达在运动，显然每一次扫描中的光束则基于不同原点进行测量，换算成笛卡尔坐标系时，显然会发现产生了畸变。因此需要进行运动补偿，即根据每个点的时刻计算其对应的原点，然后重新计算在笛卡尔坐标系下的坐标。常用的方法即采用imu等方法估计雷达运动速度，根据每条光速的时刻进行积分，可获取每条光束对应的原点坐标，进行修正。
但是TransformToStart根据他人的解释为运动补偿，即将每帧中点云均转换为以第一个扫描点为同一坐标系，即为运动补偿。但是分析其代码一直不太理解其具体意义（望理解的人分享一下）。
有人如此分析，
LeGo-LOAM源码阅读笔记(三)—featureAssociation.cpp,
// s代表的其实是一个比例，s的计算方法应该如下：
// s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD
表示无法理解，因为(pi->intensity - int(pi->intensity))为强度信息中的小数部分，小数部分应该为_scan_period * relTime，可看下面代码，这是预处理坐标系更改void adjustDistortion()中赋值。
point.intensity =int(segmentedCloud->points[i].intensity) + _scan_period * relTime;
查找多方面资料，都没有找到详细解释，望有高手分享
void FeatureAssociation::TransformToStart(PointType const *const pi,PointType *const po) {// 插值系数，为相对与第一个点的时间差// 其中10为系数，可调整，感觉像假设10是个速度？？？？？，但是速度如何来的呢float s = 10 * (pi->intensity - int(pi->intensity)); // 由于强度为thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; // 强度根据不同层则不一样，可用于显示区分// 而在特征提取进行了修改 intensity = int(segmentedCloud->points[i].intensity) + _scan_period * relTime; // 如此可看出， s应该为每扫描行中点云与第一个点云的时间差即10*_scan_period * relTime// 线性插值， 如果静止，显然无需插值，则每个点相对激光原点（或者相对于上刻位置），全部一致// 由于匀速运动，因此认为点云中的每个点相对于上刻的位置有一定的偏移，其偏移量为s// 如果s不是速度，像是个比例系数，即每个原点都在如此的比例系数下进行平移和旋转？？？？？// 不太理解，s针对一圈中固定索引的点云基本相当于一个常数// 获取了每个点的原点坐标，然后校准新的笛卡尔坐标float ry = s * transformCur[1];float rx = s * transformCur[0]; float rz = s * transformCur[2];float tx = s * transformCur[3];float ty = s * transformCur[4];float tz = s * transformCur[5];// 将该点根据自己的畸变进行旋转和平移float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);float z1 = (pi->z - tz);float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;po->x = cos(ry) * x2 - sin(ry) * z2;po->y = y2;po->z = sin(ry) * x2 + cos(ry) * z2;po->intensity = pi->intensity;
}
TransformToEnd
与TransformToStart方法一致，应该是将所有点统一到扫描的最后一点时刻的坐标系，其方法是先采用TransformToStart方法转换到第一个点，然后再转换到最后一点。
两次LM匹配
由于已经获取前后两帧的角点和平面点特征点云，因此进行帧间点云匹配，则可获取相邻两帧平移旋转变换，由初始位姿进行累计，从而获得激光里程计信息。
lego-loam采用将两次即两种特征点分别匹配，平面匹配和2维位姿匹配。
其中LM优化的流程为构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解。
平面匹配
匹配点查找
采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点，然后在找到前后两个扫描线中最近的两个点。如下图所示。其中i为当前时刻中一个点云，而m，j，l为上一帧中对应的三个点，此三个点可构成一个平面。
// 代码略，较为简单，容易理解
目标点到匹配平面的距离
匹配采用的方法为点到平面匹配，即需求点到平面的距离，具体公式截图论文。
公式不容易理解，简单解释就是求出三个点构成的平面的方向量并求出平面方程。
具体可参考点、平面和法线关系一文中有具体推导。其中法线计算为平面上两个不平行的向量叉乘。
代码中A、B、C三个点为对应的平面上的点；
- 求出两个向量AB和AC
- 计算AB和AC的叉乘，即获取方向量
- 带回平面方程，计算出平面方程常量D，获取平面方程；
- 求出方向量的模，获取单位方向量；
- 计算目标点到达法向量的投影，即为目标点到达平面的距离；
// 获取三个点，构成一个平面的三角形PointType tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; // A点PointType tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; // B点PointType tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; // C点// 平面向量// AB (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)// AC (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)// 向量叉乘获得平面法向量AB × AC// 法向量三个分量(可套公式获得)float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) -(tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) -(tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) -(tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);// 平面方程 Ax+By+Cz+D = 0 // 获取平面方程的常数Dfloat pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);// 平面法向量的模float ps = sqrt(pa * pa + pb * pb + pc * pc);// 换算成单位向量pa /= ps;pb /= ps;pc /= ps;pd /= ps;// 计算目标点到达平面的距离， 即目标点在方向量上的投影float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 考虑权重，距离越小，权重越大，应该是经验值??????float s = 1;if (iterCount >= 5) {s = 1 -1.8 * fabs(pd2) /sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y +pointSel.z * pointSel.z));}// 记录平面参数和点到平面的距离,权重过小则忽略if (s > 0.1 && pd2 != 0) {PointType coeff;coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;laserCloudOri->push_back(surfPointsFlat->points[i]); // 存放原始点云coeffSel->push_back(coeff); // 存放平面信息和点到平面距离}
LM求解
获取矩阵方程A*X=B，即求出矩阵A和B。其中A为一阶偏导数矩阵，而B即为对应点到面的距离矩阵。考虑到矩阵A不一定满秩，无法直接QR分解求解，因此矩阵方程左右两侧均左乘以A的转置。即AT∗A∗x=AT∗BA^T * A * x = A^T *BAT∗A∗x=AT∗B。然后再进行QR分解求值。
for (int i = 0; i < pointSelNum; i++) {PointType pointOri = laserCloudOri->points[i];PointType coeff = coeffSel->points[i];// 偏导数float arx =(-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x +(a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y +(a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z;float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x +(c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y +(c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z;float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;// 距离float d2 = coeff.intensity;// A 矩阵matA(i, 0) = arx;matA(i, 1) = arz;matA(i, 2) = aty;// B 矩阵matB(i, 0) = -0.05 * d2;}// A的转置matAt = matA.transpose();// A和B 同左乘A的转置， 目的是希望左侧矩阵满秩matAtA = matAt * matA;matAtB = matAt * matB;// matAtA * X = matAt * matB// QR分解求解matX = matAtA.colPivHouseholderQr().solve(matAtB);// ????不是太明白，矩阵退化判断if (iterCount == 0) {Eigen::Matrix<float,1,3> matE;Eigen::Matrix<float,3,3> matV;Eigen::Matrix<float,3,3> matV2;Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float,3,3> > esolver(matAtA);matE = esolver.eigenvalues().real();matV = esolver.eigenvectors().real();matV2 = matV;isDegenerate = false;float eignThre[3] = {10, 10, 10};for (int i = 2; i >= 0; i--) {if (matE(0, i) < eignThre[i]) {for (int j = 0; j < 3; j++) {matV2(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inverse() * matV2;}//if (isDegenerate) {Eigen::Matrix<float,3,1> matX2;matX2 = matX;matX = matP * matX2;}// 叠加每次迭代的旋转和平移量transformCur[0] += matX(0, 0);transformCur[2] += matX(1, 0);// y 轴平移 （原点云的z的平移）transformCur[4] += matX(2, 0);// 无效值就恢复0for (int i = 0; i < 6; i++) {if (std::isnan(transformCur[i])) transformCur[i] = 0;}// 计算旋转平移误差矩阵float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) +pow(RAD2DEG * (matX(1, 0)), 2));float deltaT = sqrt(pow(matX(2, 0) * 100, 2));// 旋转平移误差小于一定阈值，则停止迭代if (deltaR < 0.1 && deltaT < 0.1) {return false;}
注：1.没有认真进行偏导数的推导；2.矩阵退化没有完全理解；3.作者完全手写推导出来的，效果估计会现成的优化库快
角点匹配
角点的整个匹配与平面匹配方法完全一致，其具体实现和代码注释不再详述。
匹配点查找
采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点，然后在找到前后两个扫描线中最近的一个点。如下图所示。 其中i为当前时刻中一个点云，而j，l为上一帧中对应的两个点，此两点可构成一条直线。
目标点到直线的距离
匹配采用的是目标点到达参考直线的距离，方法如下，已知A和O两个点，求出Q到达OA直线的距离。
具体计算可参考点到直线的距离一文中有具体公式推导。
但是从loam代码中却看出计算公式如下，即求出i到直线j，l的最短距离，公式却是
d=∣ij⃗×il∣⃗∣jl⃗∣d = \frac{|\vec{ij} × \vec{il|}}{|\vec{jl}|}d=∣jl∣∣ij×il∣
按照平面法向量的思想可知，其结果一致。
odometry 发布
经过两次LM匹配，即可认为是经过帧间匹配可获取相临帧的位姿转移矩阵，通过里程计思想进行累加，则可获取当前帧的全局位姿，即获取激光里程计。
// 矩阵转换，根据变换的矩阵， 更新全局位姿
// 先旋转， 在平移
// 已知初始全局位置和相临两帧的位姿转移矩阵，
// 累加获取当前帧全局位置
void FeatureAssociation::integrateTransformation() {float rx, ry, rz, tx, ty, tz;// 旋转累加AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],-transformCur[0], -transformCur[1], -transformCur[2], rx,ry, rz);float x1 = cos(rz) * (transformCur[3] ) -sin(rz) * (transformCur[4] );float y1 = sin(rz) * (transformCur[3] ) +cos(rz) * (transformCur[4] );float z1 = transformCur[5];float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;// 平移累加tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2); ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);// 获取当前帧全局位置，即构建里程计transformSum[0] = rx;transformSum[1] = ry; // transformSum[2] = rz; // 绕z轴方向的yaw 航向角transformSum[3] = tx; // x 坐标transformSum[4] = ty; // y 坐标transformSum[5] = tz; // z 坐标
}
发布建图数据
整个特征点提取其目的包括两个：用于获取高频率的激光里程计和用于低频率的建图匹配。而获取的激光里程计频率较高，可为后期建图匹配提供预测值。可以说前3节所有处理均可认为是slam中传感器的预处理，将预处理后的数据输入slam主流程。
其预处理后的数据包括：
- 平面点云；
- 角点点云；
- 非特征点云；
- 激光里程计；
其中1,2,3三种点云的集合则为原始点云。而激光里程计也可以有其他里程计进行替换，如编码器，imu等。
其整个特征提取以及里程计处理主流程代码如下：
void FeatureAssociation::runFeatureAssociation() {while (ros::ok()) {ProjectionOut projection;_input_channel.receive(projection); // 接收三类数据if( !ros::ok() ) break;//-------------- outlierCloud = projection.outlier_cloud; //分别为未被分类的孤立点云簇segmentedCloud = projection.segmented_cloud; //被分类的包含地面的点云segInfo = std::move(projection.seg_msg); //分类的相关信息_scan_msg = std::move(projection.scan_msg);cloudHeader = segInfo.header;timeScanCur = cloudHeader.stamp.toSec();/** 1. Feature Extraction */adjustDistortion(); // 坐标系转换calculateSmoothness(); // 计算平滑性markOccludedPoints(); // 标记在水平扫描方向上，屏蔽不可靠点extractFeatures(); // 特征提取包括，角点和平坦点publishCloud(); // cloud for visualization// Feature Associationif (!systemInitedLM) { // 仅执行一次，用于初始化checkSystemInitialization();continue;}updateTransformation(); // 计算两帧激光数据之间转换矩阵integrateTransformation(); // 迭代更新，将每两帧之间变换，进行累计变换，构建里程计publishOdometry();publishCloudsLast(); // cloud to mapOptimization// 以上为高频率的激光里程计获取//--------------_cycle_count++;// 建图_mapping_frequency_div倍降频更新if (_cycle_count == _mapping_frequency_div) {_cycle_count = 0;AssociationOut out;out.cloud_corner_last.reset(new pcl::PointCloud<PointType>());out.cloud_surf_last.reset(new pcl::PointCloud<PointType>());out.cloud_outlier_last.reset(new pcl::PointCloud<PointType>());*out.cloud_corner_last = *laserCloudCornerLast; // 将平面信息 和角点发出给建图算法*out.cloud_surf_last = *laserCloudSurfLast;*out.cloud_outlier_last = *outlierCloud; // 将非特征点发给建图算法out.laser_odometry = laserOdometry; // 激光里程计}}
lego-loam代码分析（3）-激光里程计相关推荐
- 基于rf2o_laser_odometry纯激光里程计的gmapping建图
ROS环境:ubuntu16.04 & ROS kinetic 激光雷达:EAI-X4 or RPlidar-A1 激光里程计:rf2o_laser_odometry 建图:gmapping ...
- 激光SLAM源码解析S-LOAM（二）激光里程计的计算
10Hz激光雷达点云帧,相临帧的时间间隔是0.1秒.在这0.1秒内激光雷达的位姿变化(平移和旋转)可由这两帧点云的配准计算出来. 以某一时刻为起点,累计帧间位姿变换,得到各帧时刻激光雷达相对起点的位姿 ...
- 面向自动驾驶车辆的高效激光里程计（ICRA2021）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨泡泡机器人 来源丨泡泡机器人SLAM Efficient LiDAR Odometry for ...
- 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
- 多传感器融合定位（一）——3D激光里程计
目录 一.点云地图整体流程 二.激光里程计方案 2.1 ICP点到点 2.1.1 ICP推导 2.1.2 ICP改进 2.2 NDT 2.2.1 NDT推导 2.2.2 NDT改进 2.3 LOAM系 ...
- 深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
- 自动驾驶场景下的高效激光里程计
标题:Efficient LiDAR Odometry for Autonomous Driving 作者:Xin Zheng, Jianke Zhu, Senior Member, IEEE Zhe ...
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合 多传感器融合定位理论基础 文章跳转: 多传感器融合定位一-3D激光里 ...
- 多传感器融合定位二-3D激光里程计其二：NDT
多传感器融合定位二-3D激光里程计其二:NDT 1. 经典NDT 2. 计算方式 2.1 2D场景求解: 2.2 3D场景求解: 3. 其他 NDT Reference: 深蓝学院-多传感器融合 多传 ...
最新文章
- matlab p-tite分割图像,P'tite fourmi
- dvi黑屏解决方法_赛博朋克2077黑梦黑屏怎么办 黑梦BUG全黑模式解决方法
- 西北工业大学复试上机
- LWIP之IP层实现(转载)
- 典型案例道出“服务台”的价值
- 微信上传图文消息素材40007，invalid media_id hint
- startindex 不能大于字符串长度_玩转云端丨redis的5种对象与8种数据结构之字符串对象（下）...
- LeetCode 1598. 文件夹操作日志搜集器
- Quick BI产品核心功能大图（五）移动端：让数据在更多业务场景中流通
- python3下使用cv2.imwrite存储带有中文路径图片或者绝对路径图片
- t-sql 调用作业
- 一个mysql可以存多少数据类型_mysql一张表到底能存多少数据？
- python在web可以开发吗_怎么用python进行web开发
- 模板引擎工作原理_zuma致：新手SEO须知搜索引擎工作原理
- 网络安全学习第9篇 - 抓包工具wireshark使用及谈谈抓包对我们日常生活网络安全方面的威胁
- 南京邮电大学812自动控制原理高分经验
- html字体样式微软雅黑,Css样式表中实现微软雅黑字体
- matlab对5个矩阵循环求均值,MATLAB循环求数组的平均值 每隔几个数据求一下平均值...
- 执行npm install报错：npm ERR! code EINTEGRITY
- Vmware workstation 16pro解锁装MacOS