intmain(intargc,char** argv){ros::init(argc, argv,"lego_loam");ROS_INFO("\033[1;32m--->\033[0m Map Optimization Started.");mapOptimization MO;// 1.进行闭环检测与闭环的功能std::threadloopthread(&mapOptimization::loopClosur...
得到(x,y,z) 的解析式后,可以分别求取他们相对于tx,ty,ry的偏导数 对ry的偏导数为 \frac{\partial x}{\partial r_{y}}=[-cos(rz)sin(ry)-sin(rz)sin(rx)cos(ry)](x_{ori}-t_{x}) \\ +[-sin(rz)sin(ry)+cos(rz)sin(rx)cos(ry)]*(y_{ori}-t_{y}) \\ -cos(rx)cos(ry)...
Lego-Loam在工程上可能是一种state of art,确实靠谱程度高不少,手写两次lm的算法也是牛,并且结合退化逻辑可以满足更多场景。最近在改源码,主要是想用eigen库替换里面的sincos连乘,让代码可读性和可维护性变得更好,数据结构也优化一下,变量太多不好理解,另外这个代码的规范性有提高(函数、类和局部、全局变量进行修改...
IMU坐标系和Lidar坐标系说明 在LEGO-LOAM中,对激光雷达和IMU的测量轴均进行了转换,根据VLP-16的官方说明文档,如下图所示,lidar 坐标系原始的XYZ 是“右-前-上”: 激光雷达的坐标转换在 adjustDistortion() 函数中完成,见下述赋值操作代码 y->x z->y x->z point.x=segmentedCloud->points[i].y;point.y=...
本系列文章旨在解读LEGO-LOAM源码,按照代码中的各个节点,对照原文,对各个函数功能和数学公式进行了说明和推导。 整个LEGO-LOAM分为: 1) ImageProjection 2) featureAssociation 3) mapOptimization 4) TransformFusion 共四个节点,此篇文章主要介绍 1) ImageProjection 阶段的内容 ...
在每一个划分的sub-image中, 按照每个点计算的粗糙度c排序,大于阈值的点被划分为 edge point,具有最大粗糙度的edge point,不属于平面点的edge point被选择出来。 小于阈值的点被划分为 surf point,具有最小粗糙度的点,被选择出来。 对Edge Point点的处理 ...