阿里云是做网站的吗,网站代码怎么放,网页界面设计是什么,商业门户网站制作系列文章目录
【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp 【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp 【3D激光SLAM】LOAM源代码解析–laserMapping.cpp 【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp 写在前面
本系列文章将对LOAM源代码进行讲解…系列文章目录
·【3D激光SLAM】LOAM源代码解析–scanRegistration.cpp ·【3D激光SLAM】LOAM源代码解析–laserOdometry.cpp ·【3D激光SLAM】LOAM源代码解析–laserMapping.cpp ·【3D激光SLAM】LOAM源代码解析–transformMaintenance.cpp 写在前面
本系列文章将对LOAM源代码进行讲解在讲解过程中涉及到论文中提到的部分会结合论文以及我自己的理解进行解读尤其是对于其中坐标变换的部分将会进行详细的讲解。
本来是懒得写的一个是怕自己以后忘了另外是我在学习过程中其实没有感觉哪一个博主能讲解的通篇都能让我很明白特别是坐标变换部分的代码所以想着自己学完之后按照自己的理解也写一个LOAM解读希望能对后续学习LOAM的同学们有所帮助。
之后也打算录一个LOAM讲解的视频大家可以关注一下。 文章目录 系列文章目录写在前面整体框架一、变量含义二、main()函数三、接收laserMapping的转换信息四、接收laserOdometry的信息五、位姿融合总结 整体框架
LOAM多牛逼就不用多说了直接开始
先贴一下我详细注释的LOAM代码在这个版本的代码上加入了我自己的理解。
我觉得最重要也是最恶心的一部分是其中的坐标变换在代码里面真的看着头大所以先明确一下坐标系(都是右手坐标系)
IMU(IMU坐标系imu)x轴向前y轴向左z轴向上LIDAR(激光雷达坐标系l)x轴向前y轴向左z轴向上CAMERA(相机坐标系也可以理解为里程计坐标系c)z轴向前x轴向左y轴向上WORLD(世界坐标系w也叫全局坐标系与里程计第一帧init重合)z轴向前x轴向左y轴向上MAP(地图坐标系map一定程度上可以理解为里程计第一帧init)z轴向前x轴向左y轴向上
坐标变换约定 为了清晰变换矩阵的形式与《SLAM十四讲中一样》即 R A _ B R_{A\_B} RA_B表示B坐标系相对于A坐标系的变换B中一个向量通过 R A _ B R_{A\_B} RA_B可以变换到A中的向量。
首先对照ros的节点图和论文中提到的算法框架来看一下 可以看到节点图和论文中的框架是一一对应的这几个模块的功能如下
scanRegistration对原始点云进行预处理计算曲率提取特征点laserOdometry对当前sweep与上一次sweep进行特征匹配计算一个快速(10Hz)但粗略的位姿估计laserMapping对当前sweep与一个局部子图进行特征匹配计算一个慢速(1Hz)比较精确的位姿估计transformMaintenance对两个模块计算出的位姿进行融合得到最终的精确地位姿估计
本文介绍transformMaintenance模块它就是将laserOdometry和laserMapping两个模块优化得到的当前帧相对于初始帧的坐标变换进行融合从而得到最终的最优的坐标变换结果。 一、变量含义
首先介绍一下本程序用到变量的含义与laserMapping一致
transformBefMapped[6]从laserMapping模块接收到的优化前的当前帧相对于初始时刻的位姿变换 T i n i t _ e n d T_{init\_end} Tinit_endtransformSum[6]从laserOdometry模块接收到的当前帧相对于初始时刻的变换 T i n i t _ s t a r t T_{init\_start} Tinit_starttransformAftMapped[6]经过laserMapping模块优化后的当前帧相对于初始时刻的位姿变换 T m a p _ e n d T_{map\_end} Tmap_endtransformMapped[6]融合后的当前帧相对于初始帧的坐标变换
一些理解虽然transformAftMapped[6]我上面写的是 T m a p _ e n d T_{map\_end} Tmap_end看起来好像是把坐标系换成了map坐标系但是我觉得这里有两种理解都可以
AftMapped可以理解为经过laserMapping模块优化后的里程计坐标系下的当前帧end相对于初始帧的坐标变换也可以理解为经过laserMapping模块优化变到了map坐标系
二、main()函数
main函数依然很简单就是定义了一些订阅者和发布者接收/laser_odom_to_init和/aft_mapped_to_init两个坐标变换话题然后进入相应的回调函数进行融合然后发布融合后的当前帧相当于初始帧的坐标变换以及坐标变换。
int main(int argc, char** argv)
{ros::init(argc, argv, transformMaintenance);ros::NodeHandle nh;ros::Subscriber subLaserOdometry nh.subscribenav_msgs::Odometry (/laser_odom_to_init, 5, laserOdometryHandler);ros::Subscriber subOdomAftMapped nh.subscribenav_msgs::Odometry (/aft_mapped_to_init, 5, odomAftMappedHandler);ros::Publisher pubLaserOdometry2 nh.advertisenav_msgs::Odometry (/integrated_to_init, 5);pubLaserOdometry2Pointer pubLaserOdometry2;laserOdometry2.header.frame_id /camera_init;laserOdometry2.child_frame_id /camera;tf::TransformBroadcaster tfBroadcaster2;tfBroadcaster2Pointer tfBroadcaster2;laserOdometryTrans2.frame_id_ /camera_init;laserOdometryTrans2.child_frame_id_ /camera;ros::spin();return 0;
}三、接收laserMapping的转换信息
接收/aft_mapped_to_init话题的回调函数很简单就是将接收到的数据赋值给transformAftMapped[6]和transformBefMapped[6]变量这两个变量的含义与laserMapping中一致就不过多解释了。
//接收laserMapping的转换信息
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr odomAftMapped)
{double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat odomAftMapped-pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformAftMapped[0] -pitch;transformAftMapped[1] -yaw;transformAftMapped[2] roll;transformAftMapped[3] odomAftMapped-pose.pose.position.x;transformAftMapped[4] odomAftMapped-pose.pose.position.y;transformAftMapped[5] odomAftMapped-pose.pose.position.z;transformBefMapped[0] odomAftMapped-twist.twist.angular.x;transformBefMapped[1] odomAftMapped-twist.twist.angular.y;transformBefMapped[2] odomAftMapped-twist.twist.angular.z;transformBefMapped[3] odomAftMapped-twist.twist.linear.x;transformBefMapped[4] odomAftMapped-twist.twist.linear.y;transformBefMapped[5] odomAftMapped-twist.twist.linear.z;
}四、接收laserOdometry的信息
这个回调函数主要是接收到/laser_odom_to_init话题后进行先根据接收到的数据对相关变量进行赋值操作然后进入到transformAssociateToMap()函数进行位姿变换融合最后将融合后的位姿变换发布出去发布的话题为
/integrated_to_init融合后的当前帧相对于初始帧(世界坐标系)的位姿变换
另外广播了/camera相对于/camera_init的坐标变换
//接收laserOdometry的信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr laserOdometry)
{double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat laserOdometry-pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);//得到旋转平移矩阵transformSum[0] -pitch;transformSum[1] -yaw;transformSum[2] roll;transformSum[3] laserOdometry-pose.pose.position.x;transformSum[4] laserOdometry-pose.pose.position.y;transformSum[5] laserOdometry-pose.pose.position.z;transformAssociateToMap();geoQuat tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);laserOdometry2.header.stamp laserOdometry-header.stamp;laserOdometry2.pose.pose.orientation.x -geoQuat.y;laserOdometry2.pose.pose.orientation.y -geoQuat.z;laserOdometry2.pose.pose.orientation.z geoQuat.x;laserOdometry2.pose.pose.orientation.w geoQuat.w;laserOdometry2.pose.pose.position.x transformMapped[3];laserOdometry2.pose.pose.position.y transformMapped[4];laserOdometry2.pose.pose.position.z transformMapped[5];pubLaserOdometry2Pointer-publish(laserOdometry2);//发送旋转平移量laserOdometryTrans2.stamp_ laserOdometry-header.stamp;laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));tfBroadcaster2Pointer-sendTransform(laserOdometryTrans2);
}五、位姿融合
这里的位姿融合部分与laserMapping中的求解地图坐标系中end时刻到初始时刻的初始猜测–transformAssociateToMap()函数完全一致。
1.求解位移增量 transformBefMapped - transformSum的含义是上一帧相对于初始帧的位移量 与 当前帧相对于初始帧的位移量 的差值得到的结果是初始帧init坐标系下的位移增量 t i n i t s t a r t − e n d t_{init}^{start-end} tinitstart−end。
然后将其变换到end时刻 t i n i t s t a r t − e n d R e n d _ i n i t ∗ t i n i t s t a r t − e n d R i n i t _ e n d − 1 ∗ t i n i t s t a r t − e n d R i n i t _ e n d − 1 R Z X Y − 1 R − r z R − r x R − r y t_{init}^{start-end} R_{end\_init} * t_{init}^{start-end} R_{init\_end}^{-1} * t_{init}^{start-end} \\ R_{init\_end}^{-1} R_{ZXY}^{-1} R_{-rz} R_{-rx} R_{-ry} tinitstart−endRend_init∗tinitstart−endRinit_end−1∗tinitstart−endRinit_end−1RZXY−1R−rzR−rxR−ry 对应于下面代码中所示的变换。
2.求解旋转部分的融合 现在这里的变量含义分别表示为
transformSumlaserOdometry模块的当前帧相对于初始帧的变换 R i n i t _ e n d L R_{init\_end}^L Rinit_endLtransformBefMappedlaserMapping模块的当前帧相对于初始帧的变换 R i n i t _ e n d M R_{init\_end}^M Rinit_endMtransformAftMappedlaserMapping模块的优化后的当前帧相对于初始帧的变换也可以理解为当前帧相对于地图坐标系的变换 R m a p _ s t a r t M R_{map\_start}^M Rmap_startMtransformMapped融合后的当前帧相对于初始帧的坐标变换 R m a p _ e n d F R_{map\_end}^F Rmap_endF
那么有如下坐标变换关系 R m a p _ e n d F R m a p _ e n d M ∗ R i n i t _ e n d M − 1 ∗ R i n i t _ e n d L R Z X Y ∗ R Z X Y − 1 ∗ R Z X Y R_{map\_end}^F R_{map\_end}^M * R_{init\_end}^{M -1} * R_{init\_end}^L R_{ZXY} * R_{ZXY}^{-1} * R_{ZXY} Rmap_endFRmap_endM∗Rinit_endM−1∗Rinit_endLRZXY∗RZXY−1∗RZXY
这里的计算公式与laserOdometry模块中的IMU修正部分完全一样 R m a p _ e n d F [ c a c y c a c z s a c x s a c y s a c z c a c y s a c z s a c x s a c y c a c z c a c x s a c y c a c x s a c z c a c x c a c z − s a c x − s a c y c a c z s a c x c a c y s a c z s a c y s a c z s a c x c a c y c a c z c a c x c a c y ] R_{map\_end}^F\left[ \begin{matrix} cacycaczsacxsacysacz cacysaczsacxsacycacz cacxsacy\\ cacxsacz cacxcacz -sacx\\ -sacycaczsacxcacysacz sacysaczsacxcacycacz cacxcacy\\ \end{matrix} \right] Rmap_endF cacycaczsacxsacysaczcacxsacz−sacycaczsacxcacysaczcacysaczsacxsacycaczcacxcaczsacysaczsacxcacycaczcacxsacy−sacxcacxcacy R m a p _ e n d M [ c b c y c b c z s b c x s b c y s b c z c b c y s b c z s b c x s b c y c b c z c b c x s b c y c b c x s b c z c b c x c b c z − s b c x − s b c y c b c z s b c x c b c y s b c z s b c y s b c z s b c x c b c y c b c z c b c x c b c y ] R_{map\_end}^M\left[ \begin{matrix} cbcycbczsbcxsbcysbcz cbcysbczsbcxsbcycbcz cbcxsbcy\\ cbcxsbcz cbcxcbcz -sbcx\\ -sbcycbczsbcxcbcysbcz sbcysbczsbcxcbcycbcz cbcxcbcy\\ \end{matrix} \right] Rmap_endM cbcycbczsbcxsbcysbczcbcxsbcz−sbcycbczsbcxcbcysbczcbcysbczsbcxsbcycbczcbcxcbczsbcysbczsbcxcbcycbczcbcxsbcy−sbcxcbcxcbcy R i n i t _ e n d M − 1 [ c b l y c b l z − s b l x s b l y s b l z − c b l x s b l z s b l y c b l z s b l x c b l y s b l z − c b l y s b l z s b l x s b l y c b l z c b l x c b l z s b l y s b l z − s b l x c b l y c b l z − c b l x s b l y s b l x c b l x c b l y ] R_{init\_end}^{M -1}\left[ \begin{matrix} cblycblz-sblxsblysblz -cblxsblz sblycblzsblxcblysblz\\ -cblysblzsblxsblycblz cblxcblz sblysblz-sblxcblycblz\\ -cblxsbly sblx cblxcbly\\ \end{matrix} \right] Rinit_endM−1 cblycblz−sblxsblysblz−cblysblzsblxsblycblz−cblxsbly−cblxsblzcblxcblzsblxsblycblzsblxcblysblzsblysblz−sblxcblycblzcblxcbly R i n i t _ e n d L [ c a l y c a l z s a l x s a l y s a l z c a l y s a l z s a l x s a l y c a l z c a l x s a l y c a l x s a l z c a l x c a l z − s a l x − s a l y c a l z s a l x c a l y s a l z s a l y s a l z s a l x c a l y c a l z c a l x c a l y ] R_{init\_end}^L\left[ \begin{matrix} calycalzsalxsalysalz calysalzsalxsalycalz calxsaly\\ calxsalz calxcalz -salx\\ -salycalzsalxcalysalz salysalzsalxcalycalz calxcaly\\ \end{matrix} \right] Rinit_endL calycalzsalxsalysalzcalxsalz−salycalzsalxcalysalzcalysalzsalxsalycalzcalxcalzsalysalzsalxcalycalzcalxsaly−salxcalxcaly
然后使用对应位置的值相等就得到了修正后的累计变换acx、acy、acz计算如下 a c x − a r c s i n ( R 2 , 3 ) − a r c s i n ( − s b c x ∗ ( s a l x ∗ s b l x c a l x ∗ c a l y ∗ c b l x ∗ c b l y c a l x ∗ c b l x ∗ s a l y ∗ s b l y ) − c b c x ∗ c b c z ∗ ( c a l x ∗ s a l y ∗ ( c b l y ∗ s b l z − c b l z ∗ s b l x ∗ s b l y ) − c a l x ∗ c a l y ∗ ( s b l y ∗ s b l z c b l y ∗ c b l z ∗ s b l x ) c b l x ∗ c b l z ∗ s a l x ) − c b c x ∗ s b c z ∗ ( c a l x ∗ c a l y ∗ ( c b l z ∗ s b l y − c b l y ∗ s b l x ∗ s b l z ) − c a l x ∗ s a l y ∗ ( c b l y ∗ c b l z s b l x ∗ s b l y ∗ s b l z ) c b l x ∗ s a l x ∗ s b l z ) ) a c y a r c t a n ( R 1 , 3 / R 3 , 3 ) a c z a r c t a n ( R 2 , 1 / R 2 , 2 ) acx -arcsin(R_{2,3}) -arcsin(-sbcx*(salx*sblx calx*caly*cblx*cbly calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz cbly*cblz*sblx) cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz sblx*sbly*sblz) cblx*salx*sblz) ) \\ acy arctan(R_{1,3}/R_{3,3}) \\ acz arctan(R_{2,1}/R_{2,2}) acx−arcsin(R2,3)−arcsin(−sbcx∗(salx∗sblxcalx∗caly∗cblx∗cblycalx∗cblx∗saly∗sbly)−cbcx∗cbcz∗(calx∗saly∗(cbly∗sblz−cblz∗sblx∗sbly)−calx∗caly∗(sbly∗sblzcbly∗cblz∗sblx)cblx∗cblz∗salx)−cbcx∗sbcz∗(calx∗caly∗(cblz∗sbly−cbly∗sblx∗sblz)−calx∗saly∗(cbly∗cblzsblx∗sbly∗sblz)cblx∗salx∗sblz))acyarctan(R1,3/R3,3)aczarctan(R2,1/R2,2)
3.将位移增量转换到map坐标系 t m a p i n c r e m e n t R m a p _ e n d F ∗ t e n d i n c r e m e n t R m a p _ e n d F R Z X Y R y R x R z t_{map}^{increment} R_{map\_end}^F * t_{end}^{increment} \\ R_{map\_end}^F R_{ZXY} R_y R_x R_z tmapincrementRmap_endF∗tendincrementRmap_endFRZXYRyRxRz
4.求解平移部分的初始猜测 这里注意一点上面求出来的增量使用的事start时刻的累积位移减去end时刻的累计位移所以这里在求解时也是减号如下 t m a p _ e n d F t m a p _ e n d M t m a p e n d − s t a r t t m a p _ s t a r t M − t m a p s t a r t − e n d t_{map\_end}^F t_{map\_end}^M t_{map}^{end-start} t_{map\_start}^M - t_{map}^{start-end} tmap_endFtmap_endMtmapend−starttmap_startM−tmapstart−end
我在上面声明变量时提到了地图坐标系map一定程度上可以理解为里程计第一帧init这个意思就是可以理解为map坐标系和初始时刻坐标系init以及世界坐标系w是重合的而laserMapping中虽然写的是变换到了map坐标系也可以理解为仍然是当前帧end相对于初始帧init的坐标变换只是经过了laserMapping模块优化所以这里的 t m a p _ e n d F t_{map\_end}^F tmap_endF也可以写成 t i n i t _ e n d F t_{init\_end}^F tinit_endF这个解释只是为了符合作者代码中坐标变换时发布的是/camera_init到/camera的变换所以这里写 t m a p _ e n d F t_{map\_end}^F tmap_endF也没问题。
//odometry的运动估计和mapping矫正量融合之后得到的最终的位姿transformMapped
void transformAssociateToMap()
{//平移后绕y轴旋转-transformSum[1]float x1 cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 transformBefMapped[4] - transformSum[4];float z1 sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);//绕x轴旋转-transformSum[0]float x2 x1;float y2 cos(transformSum[0]) * y1 sin(transformSum[0]) * z1;float z2 -sin(transformSum[0]) * y1 cos(transformSum[0]) * z1;//绕z轴旋转-transformSum[2]transformIncre[3] cos(transformSum[2]) * x2 sin(transformSum[2]) * y2;transformIncre[4] -sin(transformSum[2]) * x2 cos(transformSum[2]) * y2;transformIncre[5] z2;float sbcx sin(transformSum[0]);float cbcx cos(transformSum[0]);float sbcy sin(transformSum[1]);float cbcy cos(transformSum[1]);float sbcz sin(transformSum[2]);float cbcz cos(transformSum[2]);float sblx sin(transformBefMapped[0]);float cblx cos(transformBefMapped[0]);float sbly sin(transformBefMapped[1]);float cbly cos(transformBefMapped[1]);float sblz sin(transformBefMapped[2]);float cblz cos(transformBefMapped[2]);float salx sin(transformAftMapped[0]);float calx cos(transformAftMapped[0]);float saly sin(transformAftMapped[1]);float caly cos(transformAftMapped[1]);float salz sin(transformAftMapped[2]);float calz cos(transformAftMapped[2]);float srx -sbcx*(salx*sblx calx*cblx*salz*sblz calx*calz*cblx*cblz)- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz sblx*sbly*sblz) cblx*salx*sbly)- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - calx*calz*(sbly*sblz cbly*cblz*sblx) cblx*cbly*salx);transformMapped[0] -asin(srx);float srycrx sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)- cblx*sblz*(caly*calz salx*saly*salz) calx*saly*sblx)- cbcx*cbcy*((caly*calz salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz) (caly*salz - calz*salx*saly)*(sbly*sblz cbly*cblz*sblx) - calx*cblx*cbly*saly) cbcx*sbcy*((caly*calz salx*saly*salz)*(cbly*cblz sblx*sbly*sblz) (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) calx*cblx*saly*sbly);float crycrx sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)- cblx*cblz*(saly*salz caly*calz*salx) calx*caly*sblx) cbcx*cbcy*((saly*salz caly*calz*salx)*(sbly*sblz cbly*cblz*sblx) (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) calx*caly*cblx*cbly)- cbcx*sbcy*((saly*salz caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly) (calz*saly - caly*salx*salz)*(cbly*cblz sblx*sbly*sblz) - calx*caly*cblx*sbly);transformMapped[1] atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0]));float srzcrx (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz cbly*cblz*sblx) cblx*cbly*salx)- (cbcy*cbcz sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz sblx*sbly*sblz) cblx*salx*sbly) cbcx*sbcz*(salx*sblx calx*cblx*salz*sblz calx*calz*cblx*cblz);float crzcrx (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz sblx*sbly*sblz) cblx*salx*sbly)- (sbcy*sbcz cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz cbly*cblz*sblx) cblx*cbly*salx) cbcx*cbcz*(salx*sblx calx*cblx*salz*sblz calx*calz*cblx*cblz);transformMapped[2] atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0]));x1 cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];y1 sin(transformMapped[2]) * transformIncre[3] cos(transformMapped[2]) * transformIncre[4];z1 transformIncre[5];x2 x1;y2 cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;z2 sin(transformMapped[0]) * y1 cos(transformMapped[0]) * z1;transformMapped[3] transformAftMapped[3] - (cos(transformMapped[1]) * x2 sin(transformMapped[1]) * z2);transformMapped[4] transformAftMapped[4] - y2;transformMapped[5] transformAftMapped[5] - (-sin(transformMapped[1]) * x2 cos(transformMapped[1]) * z2);
}
总结
到此为止整个LOAM的讲解就结束了
我的感觉就是看LOAM的论文有一种“作者说的好有道理确实就是这样啊”的感觉但是如果要是让自己想就想不出来这么牛逼的算法它的代码也写的比较漂亮。
代码的运行就不单独开一篇文章写了只要装好了依赖编译很顺畅也没报什么错我找了一个数据集测试了一下也没问题测试的数据里放在了文章开头提到的我的github仓库的bag文件夹中运行结果点云图放在了pcl文件夹中放一张结果截图。
