asp做的网站如何更新,工作总结怎么写,网站制作里的更多怎么做,医疗app开发需要多少费用系列文章目录
【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()函数以及回调函数三、求解初始猜测--transformAssociateToMap()三、将接收到的点转换到地图坐标系下四、维护局部地图五、构建周围点云地图S六、特征匹配6.1 edge point匹配6.2 planar point匹配 七、高斯牛顿优化与退化处理7.1 雅克比矩阵求解与高斯牛顿优化7.2 退化处理7.3 更新变换矩阵--transformUpdate()函数 八、将当前帧的特征点加入到地图中九、话题发布总结 整体框架
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对两个模块计算出的位姿进行融合得到最终的精确地位姿估计
本文介绍laserMapping模块它相当于SLAM的后端它维护了一个局部地图它相当于一个scan-to-map的匹配过程它的具体功能如下
接收特征点话题、全部点云话题、IMU话题并保存到对应的变量中将当前sweep与上几次sweep(局部地图)进行特征匹配得到edge point在局部子图中KD-tree查找到的5个最近点以及planar point在局部子图KD-tree查找到的5个最近点计算edge point对应5个点构成的均值和协方差矩阵对协方差矩阵进行分解求解匹配到的直线参数用planar point对应5个点构建一个超定方程组求解得到平面参数构建雅可比矩阵和残差项求解优化后的位姿变换将当前帧的点云插入局部子图维护一个局部地图用于下一帧的匹配发布话题并更新tf变换 一、变量含义
首先介绍一下本程序用到变量的含义理解这一部分非常重要
transformBefMapped[6]经过laserMapping模块优化前的上一帧相对于初始时刻的位姿变换 T i n i t _ s t a r t T_{init\_start} Tinit_starttransformSum[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_end在优化之前存储的事上一帧相对于初始时刻的位姿变换 T m a p _ s t a r t T_{map\_start} Tmap_starttransformTobeMapped[6]在laserMapping模块中用于优化的变量优化完成后会赋值给transformAftMapped[6]
一些理解虽然transformAftMapped[6]我上面写的是 T m a p _ e n d T_{map\_end} Tmap_end看起来好像是把坐标系换成了map坐标系但是我觉得这里有两种理解都可以
AftMapped可以理解为经过laserMapping模块优化后的里程计坐标系下的当前帧end相对于初始帧的坐标变换也可以理解为经过laserMapping模块优化变到了map坐标系
二、main()函数以及回调函数
main()函数是很简单的就是定义了一系列的发布者和订阅者订阅了来自laserOdometry节点发布的话题和imu数据话题然后定义了一个tf发布器发布经过mapping模块优化后的当前帧(/aft_mapped)到初始帧(/camera_init)的坐标变换然后定义了一些列下面会用到的变量。
其中有5个订阅者分别看一下它们的回调函数。
int main(int argc, char** argv)
{ros::init(argc, argv, laserMapping);ros::NodeHandle nh;ros::Subscriber subLaserCloudCornerLast nh.subscribesensor_msgs::PointCloud2(/laser_cloud_corner_last, 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast nh.subscribesensor_msgs::PointCloud2(/laser_cloud_surf_last, 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry nh.subscribenav_msgs::Odometry (/laser_odom_to_init, 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes nh.subscribesensor_msgs::PointCloud2 (/velodyne_cloud_3, 2, laserCloudFullResHandler);ros::Subscriber subImu nh.subscribesensor_msgs::Imu (/imu/data, 50, imuHandler);ros::Publisher pubLaserCloudSurround nh.advertisesensor_msgs::PointCloud2 (/laser_cloud_surround, 1);ros::Publisher pubLaserCloudFullRes nh.advertisesensor_msgs::PointCloud2 (/velodyne_cloud_registered, 2);ros::Publisher pubOdomAftMapped nh.advertisenav_msgs::Odometry (/aft_mapped_to_init, 5);nav_msgs::Odometry odomAftMapped;odomAftMapped.header.frame_id /camera_init;odomAftMapped.child_frame_id /aft_mapped;tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform aftMappedTrans;aftMappedTrans.frame_id_ /camera_init;aftMappedTrans.child_frame_id_ /aft_mapped;std::vectorint pointSearchInd;std::vectorfloat pointSearchSqDis;PointType pointOri, pointSel, pointProj, coeff;cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));bool isDegenerate false;cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));//创建VoxelGrid滤波器体素栅格滤波器pcl::VoxelGridPointType downSizeFilterCorner;//设置体素大小downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);pcl::VoxelGridPointType downSizeFilterSurf;downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);pcl::VoxelGridPointType downSizeFilterMap;downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);//指针初始化for (int i 0; i laserCloudNum; i) {laserCloudCornerArray[i].reset(new pcl::PointCloudPointType());laserCloudSurfArray[i].reset(new pcl::PointCloudPointType());laserCloudCornerArray2[i].reset(new pcl::PointCloudPointType());laserCloudSurfArray2[i].reset(new pcl::PointCloudPointType());}int frameCount stackFrameNum - 1; //0 stackFrameNum 1 这个改成10才是论文中的1Hz频率int mapFrameCount mapFrameNum - 1; //4 mapFrameNum 5ros::Rate rate(100);bool status ros::ok();接收特征点的回调函数
下面这三个回调函数的作用和代码结构都类似都是接收laserOdometry发布的话题分别接收了less edge pointless planar point、full cloud point。
对于接收到点云之后都是如下操作
记录时间戳保存到相应变量中将接收标志设置为true
//接收边沿点
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr laserCloudCornerLast2)
{timeLaserCloudCornerLast laserCloudCornerLast2-header.stamp.toSec();laserCloudCornerLast-clear();pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);newLaserCloudCornerLast true;
}//接收平面点
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr laserCloudSurfLast2)
{timeLaserCloudSurfLast laserCloudSurfLast2-header.stamp.toSec();laserCloudSurfLast-clear();pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);newLaserCloudSurfLast true;
}//接收点云全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr laserCloudFullRes2)
{timeLaserCloudFullRes laserCloudFullRes2-header.stamp.toSec();laserCloudFullRes-clear();pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);newLaserCloudFullRes true;
}接收IMU原始数据/imu_data的消息 这个回调函数主要是接收了IMU原始数据话题更新最新的IMU数据指针imuPointerLast记录下imu的时间戳然后这里使用了roll和pitch两个角。
//接收IMU信息只使用了翻滚角和俯仰角
void imuHandler(const sensor_msgs::Imu::ConstPtr imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn-orientation, orientation);tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);imuPointerLast (imuPointerLast 1) % imuQueLength;imuTime[imuPointerLast] imuIn-header.stamp.toSec();imuRoll[imuPointerLast] roll;imuPitch[imuPointerLast] pitch;
}接收laserOdometry计算得到的位姿变换
接收laserOdometry模块计算出的当前帧相end相对于初始帧init的坐标变换并将其保存在transformSum变量中。
//接收旋转平移信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr laserOdometry)
{timeLaserOdometry laserOdometry-header.stamp.toSec();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;newLaserOdometry true;
}三、求解初始猜测–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.求解旋转部分的初始猜测 现在这里的变量含义分别表示为
transformSum当前帧相对于初始帧的变换 R i n i t _ e n d R_{init\_end} Rinit_endtransformBefMapped上一帧相对于初始帧的变换 R i n i t _ s t a r t R_{init\_start} Rinit_starttransformAftMapped上一帧优化后的相对于初始帧的变换也可以理解为上一帧相对于地图坐标系的变换 R m a p _ s t a r t R_{map\_start} Rmap_starttransformTobeMapped当前帧相对于初始帧的初始猜测也可以理解为当前帧相对于地图坐标系的变换 R ~ m a p _ e n d \tilde R_{map\_end} R~map_end
那么有如下坐标变换关系 R ~ m a p _ e n d R m a p _ s t a r t ∗ R i n i t _ s t a r t − 1 ∗ R i n i t _ e n d R Z X Y ∗ R Z X Y − 1 ∗ R Z X Y \tilde R_{map\_end} R_{map\_start} * R_{init\_start}^{-1} * R_{init\_end} R_{ZXY} * R_{ZXY}^{-1} * R_{ZXY} R~map_endRmap_start∗Rinit_start−1∗Rinit_endRZXY∗RZXY−1∗RZXY
这里的计算公式与laserOdometry模块中的IMU修正部分完全一样 R ~ m a p _ e n d [ 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 ] \tilde R_{map\_end}\left[ \begin{matrix} cacycaczsacxsacysacz cacysaczsacxsacycacz cacxsacy\\ cacxsacz cacxcacz -sacx\\ -sacycaczsacxcacysacz sacysaczsacxcacycacz cacxcacy\\ \end{matrix} \right] R~map_end cacycaczsacxsacysaczcacxsacz−sacycaczsacxcacysaczcacysaczsacxsacycaczcacxcaczsacysaczsacxcacycaczcacxsacy−sacxcacxcacy R m a p _ s t a r t [ 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\_start}\left[ \begin{matrix} cbcycbczsbcxsbcysbcz cbcysbczsbcxsbcycbcz cbcxsbcy\\ cbcxsbcz cbcxcbcz -sbcx\\ -sbcycbczsbcxcbcysbcz sbcysbczsbcxcbcycbcz cbcxcbcy\\ \end{matrix} \right] Rmap_start cbcycbczsbcxsbcysbczcbcxsbcz−sbcycbczsbcxcbcysbczcbcysbczsbcxsbcycbczcbcxcbczsbcysbczsbcxcbcycbczcbcxsbcy−sbcxcbcxcbcy R i n i t _ s t a r t − 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\_start}^{-1}\left[ \begin{matrix} cblycblz-sblxsblysblz -cblxsblz sblycblzsblxcblysblz\\ -cblysblzsblxsblycblz cblxcblz sblysblz-sblxcblycblz\\ -cblxsbly sblx cblxcbly\\ \end{matrix} \right] Rinit_start−1 cblycblz−sblxsblysblz−cblysblzsblxsblycblz−cblxsbly−cblxsblzcblxcblzsblxsblycblzsblxcblysblzsblysblz−sblxcblycblzcblxcbly R i n i t _ e n d [ 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}\left[ \begin{matrix} calycalzsalxsalysalz calysalzsalxsalycalz calxsaly\\ calxsalz calxcalz -salx\\ -salycalzsalxcalysalz salysalzsalxcalycalz calxcaly\\ \end{matrix} \right] Rinit_end 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 ∗ t e n d i n c r e m e n t R ~ m a p _ e n d R Z X Y R y R x R z t_{map}^{increment} \tilde R_{map\_end} * t_{end}^{increment} \\ \tilde R_{map\_end} R_{ZXY} R_y R_x R_z tmapincrementR~map_end∗tendincrementR~map_endRZXYRyRxRz
4.求解平移部分的初始猜测 这里注意一点上面求出来的增量使用的事start时刻的累积位移减去end时刻的累计位移所以这里在求解时也是减号如下 t ~ m a p _ e n d t m a p _ s t a r t t m a p e n d − s t a r t t m a p _ s t a r t − t m a p s t a r t − e n d \tilde t_{map\_end} t_{map\_start} t_{map}^{end-start} t_{map\_start} - t_{map}^{start-end} t~map_endtmap_starttmapend−starttmap_start−tmapstart−end
//基于匀速模型根据上次微调的结果和odometry这次与上次计算的结果猜测一个新的世界坐标系的转换矩阵transformTobeMapped
// 得到T_map_end的初始猜测transformTobeMapped
void transformAssociateToMap()
{// 这个写的很好https://zhuanlan.zhihu.com/p/159525107// 这里用到的旋转矩阵R_end_init R_init_end^{-1} R_ZXY^{-1} R_-rz * R_-rx * R_-ry// 这里的运算把括号拆开看transformIncre t_end^{start-end} R_end_init * t_init^{start-end}// R_end_init * (t_init_start - t_init_end)//绕y轴旋转 -ryfloat 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轴旋转 -rxfloat x2 x1;float y2 cos(transformSum[0]) * y1 sin(transformSum[0]) * z1;float z2 -sin(transformSum[0]) * y1 cos(transformSum[0]) * z1;//绕z轴旋转 -rz//平移增量transformIncre[3] cos(transformSum[2]) * x2 sin(transformSum[2]) * y2;transformIncre[4] -sin(transformSum[2]) * x2 cos(transformSum[2]) * y2;transformIncre[5] z2;// T_init_endfloat 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]);// T_init_startfloat 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]);//transformAftMapped是上一次构图经过mapping微调后的转换矩阵//可以理解为上一次的构图帧最终解算的位姿 T_map_startfloat 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]);// R_transformTobeMapped// 初始猜测R_map_end // R_map_start * (R_init_start)^{-1} * R_init_end// R_transformAftMapped * (R_transformBefMapped)^{-1} * R_transformSumfloat 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);transformTobeMapped[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);transformTobeMapped[1] atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[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);transformTobeMapped[2] atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0]));//这样就得到了一个世界系到当前构图帧的不错的初始姿态存入transformTobeMapped中用作迭代匹配的初值// 转换到map坐标系// 这里用到的旋转矩阵R_map_end R_ZXY R_rz * R_rx * R_ry// 把位移增量转换到map坐标系下t_map^{start-end} R_map_end * t_end^{start-end}//绕z轴旋转 rzx1 cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];y1 sin(transformTobeMapped[2]) * transformIncre[3] cos(transformTobeMapped[2]) * transformIncre[4];z1 transformIncre[5];//绕x轴旋转 rxx2 x1;y2 cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;z2 sin(transformTobeMapped[0]) * y1 cos(transformTobeMapped[0]) * z1;//绕y轴旋转 rytransformTobeMapped[3] transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 sin(transformTobeMapped[1]) * z2);transformTobeMapped[4] transformAftMapped[4] - y2;transformTobeMapped[5] transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 cos(transformTobeMapped[1]) * z2);// t_map_end t_map_start - t_map^{start-end}
}三、将接收到的点转换到地图坐标系下
这一部分主要的功能就是判断一下时间戳和是否接受到该帧的特征点如果接收到了的好就根据初始猜测将接收到的点从end时刻坐标系变换到map坐标系下主要函数就是pointAssociateToMap()函数 while (status) {ros::spinOnce();if (newLaserCloudCornerLast newLaserCloudSurfLast newLaserCloudFullRes newLaserOdometry fabs(timeLaserCloudCornerLast - timeLaserOdometry) 0.005 fabs(timeLaserCloudSurfLast - timeLaserOdometry) 0.005 fabs(timeLaserCloudFullRes - timeLaserOdometry) 0.005) {newLaserCloudCornerLast false;newLaserCloudSurfLast false;newLaserCloudFullRes false;newLaserOdometry false;frameCount;//控制跳帧数这里实际并没有跳帧只取或者增大stackFrameNum才能实现相应的跳帧处理if (frameCount stackFrameNum) {//获取T_map_end的初始猜测transformAssociateToMap();//将最新接收到的平面点和边沿点进行旋转平移转换到地图坐标系map下int laserCloudCornerLastNum laserCloudCornerLast-points.size();for (int i 0; i laserCloudCornerLastNum; i) {pointAssociateToMap(laserCloudCornerLast-points[i], pointSel);laserCloudCornerStack2-push_back(pointSel);}int laserCloudSurfLastNum laserCloudSurfLast-points.size();for (int i 0; i laserCloudSurfLastNum; i) {pointAssociateToMap(laserCloudSurfLast-points[i], pointSel);laserCloudSurfStack2-push_back(pointSel);}}pointAssociateToMap()函数 输入的pi是end时刻坐标系下的点这个变换过程如下式 p m a p R ~ m a p _ e n d ∗ p e n d t m a p _ e n d R y R x R z ∗ p e n d t m a p _ e n d p_{map} \tilde R_{map\_end} * p_{end} t_{map\_end} R_y R_x R_z * p_{end} t_{map\_end} pmapR~map_end∗pendtmap_endRyRxRz∗pendtmap_end
//根据调整计算后的转移矩阵将点注册到全局世界坐标系下
void pointAssociateToMap(PointType const * const pi, PointType * const po)
{//绕z轴旋转transformTobeMapped[2]float x1 cos(transformTobeMapped[2]) * pi-x- sin(transformTobeMapped[2]) * pi-y;float y1 sin(transformTobeMapped[2]) * pi-x cos(transformTobeMapped[2]) * pi-y;float z1 pi-z;//绕x轴旋转transformTobeMapped[0]float x2 x1;float y2 cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;float z2 sin(transformTobeMapped[0]) * y1 cos(transformTobeMapped[0]) * z1;//绕y轴旋转transformTobeMapped[1]再平移po-x cos(transformTobeMapped[1]) * x2 sin(transformTobeMapped[1]) * z2 transformTobeMapped[3];po-y y2 transformTobeMapped[4];po-z -sin(transformTobeMapped[1]) * x2 cos(transformTobeMapped[1]) * z2 transformTobeMapped[5];po-intensity pi-intensity;
}四、维护局部地图
首先前面求解了一个在y方向上10米高位置的点在世界坐标系下的坐标这个主要是为了后面帮助判断周围选定的周围点云集合S中的cube是否在激光雷达的视域内可以先不用管它之后我们再讲。
下面先声明一些局部地图中使用到的变量
laserCloudCornerArray存放edge point的总地图按照论文的话就是所有cube的总集合一共有laserCloudNum211121个cube一个cube尺寸为50x50x50米laserCloudSurfArray存放planar point的总地图按照论文的话就是所有cube的总集合一共有laserCloudNum211121个cube一个cube尺寸为50x50x50米laserCloudWidth宽度方向cube数量//21laserCloudHeight高度方向cube数量//11laserCloudDepth深度方向cube数量//21laserCloudNum总cube数量 laserCloudWidth * laserCloudHeight * laserCloudDepth;//21x11x21laserCloudCenWidth地图中心点在宽度方向的偏移量//(211)/2laserCloudCenHeight地图中心点在高度方向的偏移量//(111)/2laserCloudCenDepth地图中心点在深度方向的偏移量//(211)/2laserCloudSurroundInd构成周围点云cube集合的索引对应论文中构成S的cube的索引laserCloudValidInd构成周围点云cube集合并且在激光雷达视域范围内的索引centerCubeI机器人位置在地图中宽度方向的cube索引centerCubeJ机器人位置在地图中高度方向的cube索引centerCubeK机器人位置在地图中深度方向的cube索引
下面就是求解机器人位置的cube索引centerCubeI、centerCubeJ、centerCubeK下图源自SLAM前端入门框架-A_LOAM源码解析-知乎 然后下面六个while循环就是循环移位的过程如果机器人的位置靠近了地图边缘那就调整地图的中心靠近机器人的位置比如第一个while循环意思是如果centerCubeI3说明机器人位置处于下边界表明地图向负方向延伸的可能性比较大则循环移位将数组中心点向上边界调整一个单位。
调整后需要满足 3 centerCubeI 18 3 centerCubeJ 8, 3 centerCubeK 18 if (frameCount stackFrameNum) {frameCount 0;PointType pointOnYAxis;pointOnYAxis.x 0.0;pointOnYAxis.y 10.0;pointOnYAxis.z 0.0;//获取y方向上10米高位置的点在世界坐标系下的坐标pointAssociateToMap(pointOnYAxis, pointOnYAxis);//立方体的中点原点在世界坐标系下的位置//过半取一以50米进行四舍五入的效果由于数组下标只能为正数而地图可能建立在原点前后因此//每一维偏移一个laserCloudCenWidth该值会动态调整以使得数组利用最大化初始值为该维数组长度1/2的量int centerCubeI int((transformTobeMapped[3] 25.0) / 50.0) laserCloudCenWidth;int centerCubeJ int((transformTobeMapped[4] 25.0) / 50.0) laserCloudCenHeight;int centerCubeK int((transformTobeMapped[5] 25.0) / 50.0) laserCloudCenDepth;//由于计算机求余是向零取整为了不使-50.0,50.0求余后都向零偏移当被求余数为负数时求余结果统一向左偏移一个单位也即减一if (transformTobeMapped[3] 25.0 0) centerCubeI--;if (transformTobeMapped[4] 25.0 0) centerCubeJ--;if (transformTobeMapped[5] 25.0 0) centerCubeK--;//调整之后取值范围:3 centerCubeI 18 3 centerCubeJ 8, 3 centerCubeK 18//如果处于下边界表明地图向负方向延伸的可能性比较大则循环移位将数组中心点向上边界调整一个单位while (centerCubeI 3) {for (int j 0; j laserCloudHeight; j) {for (int k 0; k laserCloudDepth; k) {//实现一次循环移位效果int i laserCloudWidth - 1;//指针赋值保存最后一个指针位置pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//thats [i 21 * j 231 * k]pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//循环移位I维度上依次后移for (; i 1; i--) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i - 1 laserCloudWidth*j laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i - 1 laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];}//将开始点赋值为最后一个点laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeI;laserCloudCenWidth;}//如果处于上边界表明地图向正方向延伸的可能性比较大则循环移位将数组中心点向下边界调整一个单位while (centerCubeI laserCloudWidth - 3) {//18for (int j 0; j laserCloudHeight; j) {for (int k 0; k laserCloudDepth; k) {int i 0;pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//I维度上依次前移for (; i laserCloudWidth - 1; i) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i 1 laserCloudWidth*j laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i 1 laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeI--;laserCloudCenWidth--;}while (centerCubeJ 3) {for (int i 0; i laserCloudWidth; i) {for (int k 0; k laserCloudDepth; k) {int j laserCloudHeight - 1;pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//J维度上依次后移for (; j 1; j--) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i laserCloudWidth*(j - 1) laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i laserCloudWidth * (j - 1) laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeJ;laserCloudCenHeight;} while (centerCubeJ laserCloudHeight - 3) {for (int i 0; i laserCloudWidth; i) {for (int k 0; k laserCloudDepth; k) {int j 0;pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//J维度上一次前移for (; j laserCloudHeight - 1; j) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i laserCloudWidth*(j 1) laserCloudWidth * laserCloudHeight*k];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i laserCloudWidth * (j 1) laserCloudWidth * laserCloudHeight*k];}laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeJ--;laserCloudCenHeight--;}while (centerCubeK 3) {for (int i 0; i laserCloudWidth; i) {for (int j 0; j laserCloudHeight; j) {int k laserCloudDepth - 1;pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//K维度上依次后移for (; k 1; k--) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i laserCloudWidth*j laserCloudWidth * laserCloudHeight*(k - 1)];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight*(k - 1)];}laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeK;laserCloudCenDepth;}while (centerCubeK laserCloudDepth - 3) {for (int i 0; i laserCloudWidth; i) {for (int j 0; j laserCloudHeight; j) {int k 0;pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];//K维度上依次前移for (; k laserCloudDepth - 1; k) {laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i laserCloudWidth*j laserCloudWidth * laserCloudHeight*(k 1)];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight*(k 1)];}laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}centerCubeK--;laserCloudCenDepth--;}
五、构建周围点云地图S
这一部分对应于论文中下面这一段 以机器人当前位置所处的cube为中心取宽度方向5个、深度方向5个、高度方向5个前后250米范围内总共500米范围三个维度总共125个cube然后在这125个cube里面进一步筛选在视域范围内的cube。
筛选方法如下
先将周围125个cube的每一个cube索引换算成在世界坐标系下的坐标(以米为单位)取周围125个cube的每一个cube的上下左右八个顶点坐标求原点到顶点距离的平方和机器人上方10m一个点pointOnYAxis到顶点距离的平方应用余弦定理求解角度如果在(30,150)范围内则认为在视域内图示如下 如果check10并且check20就说明图示中的θ角在30度到150度之间判定为在激光雷达视域范围内。
如果某个cube在视域内就将该cube的索引保存到laserCloudValidInd[]数组中然后不管是不是在视域内都将cube的索引保存到laserCloudSurroundInd[]数组中
最后再通过一个for循环使用在视域范围内的cube构建周围点云S代码中为laserCloudCornerFromMap和laserCloudSurfFromMap点云 //下面这个三层for循环用来寻找构成局部点云集合S的cubeint laserCloudValidNum 0; // 局部地图有效(在视野范围内)cube数量int laserCloudSurroundNum 0; // 局部地图所有cube数量//在每一维附近5个cube(前2个后2个中间1个)里进行查找前后250米范围内总共500米范围三个维度总共125个cube//在这125个cube里面进一步筛选在视域范围内的cubefor (int i centerCubeI - 2; i centerCubeI 2; i) {for (int j centerCubeJ - 2; j centerCubeJ 2; j) {for (int k centerCubeK - 2; k centerCubeK 2; k) {if (i 0 i laserCloudWidth j 0 j laserCloudHeight k 0 k laserCloudDepth) {//如果索引合法//换算成实际比例在世界坐标系下的坐标float centerX 50.0 * (i - laserCloudCenWidth);float centerY 50.0 * (j - laserCloudCenHeight);float centerZ 50.0 * (k - laserCloudCenDepth);//下面的代码是用来判断周围点云集合S的cube是否在雷达的视线范围内bool isInLaserFOV false;for (int ii -1; ii 1; ii 2) {for (int jj -1; jj 1; jj 2) {for (int kk -1; kk 1; kk 2) {//周围点云S中的cube的上下左右八个顶点坐标float cornerX centerX 25.0 * ii;float cornerY centerY 25.0 * jj;float cornerZ centerZ 25.0 * kk;//原点到顶点距离的平方float squaredSide1 (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX) (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY) (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ);//pointOnYAxis到顶点距离的平方float squaredSide2 (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY) (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);//用了余弦定理check1其实是机器人原点位置与cube中点的连线 与 机器人上方10m线段 的夹角 的余弦 - cos30//如果check10说明这个夹角大于30度在视野范围内float check1 100.0 squaredSide1 - squaredSide2- 10.0 * sqrt(3.0) * sqrt(squaredSide1);//check2其实是机器人原点位置与cube中点的连线 与 机器人上方10m线段 的夹角 的余弦 - cos150//如果check20说明这个夹角小于150度在视野范围内float check2 100.0 squaredSide1 - squaredSide2 10.0 * sqrt(3.0) * sqrt(squaredSide1);//其实就是激光雷达的视野(-30,30)if (check1 0 check2 0) {isInLaserFOV true;}}}}//记住视域范围内的cube索引匹配用if (isInLaserFOV) {laserCloudValidInd[laserCloudValidNum] i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k;laserCloudValidNum;}//记住附近所有cube的索引显示用laserCloudSurroundInd[laserCloudSurroundNum] i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k;laserCloudSurroundNum;}}}}laserCloudCornerFromMap-clear();laserCloudSurfFromMap-clear();//构建特征点地图查找匹配使用对应于论文中S的部分for (int i 0; i laserCloudValidNum; i) {*laserCloudCornerFromMap *laserCloudCornerArray[laserCloudValidInd[i]];*laserCloudSurfFromMap *laserCloudSurfArray[laserCloudValidInd[i]];}六、特征匹配
首先将点云又变回局部坐标系也就是里程计坐标系下的end时刻个人没看懂这个操作有什么意义前面变到map坐标系中也没进行什么操作。
对视域范围内的周围点云S进行降采样操作以减少运算量然后如果周围点云中edge point数量大于10并且planar point数量大于100则开始进行特征匹配。
6.1 edge point匹配
用S构造KD-tree搜索与待匹配点最近的5个点求这5个点的均值保存在cx、cy、cz中求这5个点构成的协方差矩阵保存在matA1矩阵中然后对matA1进行特征值分解。
接下来就是判断这五个点是否构成一条直线按照论文所述如果三个特征值中有一个特征值明显大于其他两个则认为这5个点构成一条直线且该特征值对应的特征向量为构成直线的方向向量在代码中当最大特征大于次大特征值3倍以上则认为构成直线。
在均值点处沿着方向向量乘以0.1和-0.1分别构造出该直线上两个点即代码中的(x1,y1,z1)和(x2,y2,z2)然后类似于laserOdometry中的方法求解待匹配点到直线的距离以及垂线向量在xyz轴三个方向的分量保存到coeff中匹配成功的点放入laserCloudOri中。 int laserCloudCornerFromMapNum laserCloudCornerFromMap-points.size();int laserCloudSurfFromMapNum laserCloudSurfFromMap-points.size();/***********************************************************************此处将特征点转移回local坐标系是为了voxel grid filter的下采样操作不越界好像不是后面还会转移回世界坐标系这里是前面的逆转换和前面一样应无必要可直接对laserCloudCornerLast和laserCloudSurfLast进行下采样***********************************************************************/int laserCloudCornerStackNum2 laserCloudCornerStack2-points.size();for (int i 0; i laserCloudCornerStackNum2; i) {pointAssociateTobeMapped(laserCloudCornerStack2-points[i], laserCloudCornerStack2-points[i]);}int laserCloudSurfStackNum2 laserCloudSurfStack2-points.size();for (int i 0; i laserCloudSurfStackNum2; i) {pointAssociateTobeMapped(laserCloudSurfStack2-points[i], laserCloudSurfStack2-points[i]);}// 为了减少运算量对点云进行下采样laserCloudCornerStack-clear();downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);//设置滤波对象downSizeFilterCorner.filter(*laserCloudCornerStack);//执行滤波处理int laserCloudCornerStackNum laserCloudCornerStack-points.size();//获取滤波后体素点尺寸laserCloudSurfStack-clear();downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);downSizeFilterSurf.filter(*laserCloudSurfStack);int laserCloudSurfStackNum laserCloudSurfStack-points.size();laserCloudCornerStack2-clear();laserCloudSurfStack2-clear();//如果点云数量足够大if (laserCloudCornerFromMapNum 10 laserCloudSurfFromMapNum 100) {kdtreeCornerFromMap-setInputCloud(laserCloudCornerFromMap);//构建kd-treekdtreeSurfFromMap-setInputCloud(laserCloudSurfFromMap);for (int iterCount 0; iterCount 10; iterCount) {//最多迭代10次laserCloudOri-clear();coeffSel-clear();for (int i 0; i laserCloudCornerStackNum; i) {pointOri laserCloudCornerStack-points[i];//转换回世界坐标系pointAssociateToMap(pointOri, pointSel);kdtreeCornerFromMap-nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);//寻找最近距离五个点if (pointSearchSqDis[4] 1.0) {//5个点中最大距离不超过1才处理//将五个最近点的坐标加和求平均float cx 0;float cy 0; float cz 0;for (int j 0; j 5; j) {cx laserCloudCornerFromMap-points[pointSearchInd[j]].x;cy laserCloudCornerFromMap-points[pointSearchInd[j]].y;cz laserCloudCornerFromMap-points[pointSearchInd[j]].z;}cx / 5;cy / 5; cz / 5;//求均方差float a11 0;float a12 0; float a13 0;float a22 0;float a23 0; float a33 0;for (int j 0; j 5; j) {float ax laserCloudCornerFromMap-points[pointSearchInd[j]].x - cx;float ay laserCloudCornerFromMap-points[pointSearchInd[j]].y - cy;float az laserCloudCornerFromMap-points[pointSearchInd[j]].z - cz;a11 ax * ax;a12 ax * ay;a13 ax * az;a22 ay * ay;a23 ay * az;a33 az * az;}a11 / 5;a12 / 5; a13 / 5;a22 / 5;a23 / 5; a33 / 5;//构建协方差矩阵matA1.atfloat(0, 0) a11;matA1.atfloat(0, 1) a12;matA1.atfloat(0, 2) a13;matA1.atfloat(1, 0) a12;matA1.atfloat(1, 1) a22;matA1.atfloat(1, 2) a23;matA1.atfloat(2, 0) a13;matA1.atfloat(2, 1) a23;matA1.atfloat(2, 2) a33;//特征值分解cv::eigen(matA1, matD1, matV1);// 论文中最大特征值大于次大特征值的3倍认为是线特征if (matD1.atfloat(0, 0) 3 * matD1.atfloat(0, 1)) {//如果最大的特征值大于第二大的特征值三倍以上float x0 pointSel.x;float y0 pointSel.y;float z0 pointSel.z;float x1 cx 0.1 * matV1.atfloat(0, 0);float y1 cy 0.1 * matV1.atfloat(0, 1);float z1 cz 0.1 * matV1.atfloat(0, 2);float x2 cx - 0.1 * matV1.atfloat(0, 0);float y2 cy - 0.1 * matV1.atfloat(0, 1);float z2 cz - 0.1 * matV1.atfloat(0, 2);float a012 sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); //OA、OB叉乘的模float l12 sqrt((x1 - x2)*(x1 - x2) (y1 - y2)*(y1 - y2) (z1 - z2)*(z1 - z2)); //AB的模//O到AB的垂线的方向向量在三个方向的分量float la ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float ld2 a012 / l12;//unusedpointProj pointSel;pointProj.x - la * ld2;pointProj.y - lb * ld2;pointProj.z - lc * ld2;//权重系数计算float s 1 - 0.9 * fabs(ld2);coeff.x s * la;coeff.y s * lb;coeff.z s * lc;coeff.intensity s * ld2;if (s 0.1) {//距离足够小才使用laserCloudOri-push_back(pointOri);coeffSel-push_back(coeff);}}}}6.2 planar point匹配
用S构造KD-tree搜索与待匹配点最近的5个点用这5个点构建了一个超定方程组用来求解平面方程Ax By Cz 1 0的ABCD系数尝试去拟合一个平面。
接下来就是判断这五个点拟合的这个平面好不好论文中的判断方法是如果有一个特征值远远小于其他两个特征值那么这个特征值对应的特征向量为平面的法向量但是在代码中并没有使用这种方式。 代码中的方法是求解用于拟合的5个点到拟合出的平面的距离如果有任何一个距离大于0.2米则认为平面拟合的不好将不进行匹配否则将记录下平面参数和待匹配点到平面的距离保存在coeffSel中将匹配成功的点保存到laserCloudOri中。 for (int i 0; i laserCloudSurfStackNum; i) {pointOri laserCloudSurfStack-points[i];pointAssociateToMap(pointOri, pointSel); kdtreeSurfFromMap-nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);if (pointSearchSqDis[4] 1.0) {// 构建平面方程Ax By Cz 1 0// 通过构建一个超定方程来求解这个平面方程for (int j 0; j 5; j) {matA0.atfloat(j, 0) laserCloudSurfFromMap-points[pointSearchInd[j]].x;matA0.atfloat(j, 1) laserCloudSurfFromMap-points[pointSearchInd[j]].y;matA0.atfloat(j, 2) laserCloudSurfFromMap-points[pointSearchInd[j]].z;}//求解matA0*matX0matB0cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);float pa matX0.atfloat(0, 0);float pb matX0.atfloat(1, 0);float pc matX0.atfloat(2, 0);float pd 1;float ps sqrt(pa * pa pb * pb pc * pc);pa / ps;pb / ps;pc / ps;pd / ps;bool planeValid true;// 点如果距离平面太远就认为这是一个拟合的不好的平面for (int j 0; j 5; j) {if (fabs(pa * laserCloudSurfFromMap-points[pointSearchInd[j]].x pb * laserCloudSurfFromMap-points[pointSearchInd[j]].y pc * laserCloudSurfFromMap-points[pointSearchInd[j]].z pd) 0.2) {planeValid false;break;}}if (planeValid) {float pd2 pa * pointSel.x pb * pointSel.y pc * pointSel.z pd;//unusedpointProj pointSel;pointProj.x - pa * pd2;pointProj.y - pb * pd2;pointProj.z - pc * pd2;float s 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x pointSel.y * pointSel.y pointSel.z * pointSel.z));coeff.x s * pa;coeff.y s * pb;coeff.z s * pc;coeff.intensity s * pd2;if (s 0.1) {laserCloudOri-push_back(pointOri);coeffSel-push_back(coeff);}}}}
七、高斯牛顿优化与退化处理
这一部分与laserOdometry完全一样直接复制过来啦
7.1 雅克比矩阵求解与高斯牛顿优化
在代码中作者是纯手推的高斯牛顿算法这种方式相比于使用Ceres等工具会提高运算速度但是计算雅克比矩阵比较麻烦需要清晰的思路和扎实的数学功底下面我们一起来推导一下。
以edge point匹配为例planar point是一样的。
设误差函数(点到直线的距离)为 f ( X ) D ( p s t a r t i , p s t a r t t ) f(X)D(p_{start}^i,p_{start}^t) f(X)D(pstarti,pstartt) 其中X为待优化变量也就是transform[6]中存储的变量表示3轴旋转rx、ry、rz和3轴平移量tx、ty、tzD()表示两点之间的距离其计算公式为 D ( p s t a r t i , p s t a r t t ) ( p s t a r t i − p s t a r t t ) T ( p s t a r t i − p s t a r t t ) D(p_{start}^i,p_{start}^t) \sqrt {(p_{start}^i-p_{start}^t)^T (p_{start}^i-p_{start}^t) } D(pstarti,pstartt)(pstarti−pstartt)T(pstarti−pstartt) p s t a r t i p_{start}^i pstarti表示start时刻坐标系下待匹配点i p s t a r t t p_{start}^t pstartt表示start时刻坐标系下点i到直线jl的垂点另外根据之前TransformToStart()函数推导过的坐标变换有 p s t a r t i R c u r r _ s t a r t − 1 ∗ ( p c u r r − 1 − t c u r r _ s t a r t ) [ c r y c r z − s r x s r y s r z − c r y s r z s r x s r y c r z − c r x s r y − c r x s r z c r x c r z s r x s r y c r z s r x c r y s r z s r y s r z − s r x c r y c r z c r x c r y ] ⋅ [ p c u r r − t x p c u r r − t y p c u r r − t z ] p_{start}^i R_{curr\_start}^{-1}*(p_{curr}^{-1}-t_{curr\_start}) \left[ \begin{matrix} crycrz-srxsrysrz -crysrzsrxsrycrz -crxsry\\ -crxsrz crxcrz srx\\ srycrzsrxcrysrz srysrz-srxcrycrz crxcry\\ \end{matrix} \right] \cdot \left[ \begin{array}{c} p_{curr}-tx\\ p_{curr}-ty\\ p_{curr}-tz\\ \end{array} \right] pstartiRcurr_start−1∗(pcurr−1−tcurr_start) crycrz−srxsrysrz−crxsrzsrycrzsrxcrysrz−crysrzsrxsrycrzcrxcrzsrysrz−srxcrycrz−crxsrysrxcrxcry ⋅ pcurr−txpcurr−typcurr−tz
根据链式法则f(x)对X求导有 ∂ f ( x ) ∂ X ∂ f ( x ) ∂ D ( p s t a r t i , p s t a r t t ) ∗ ∂ D ( p s t a r t i , p s t a r t t ) ∂ p s t a r t i ∗ ∂ p s t a r t i ∂ X \frac{∂f(x)}{∂X} \frac{∂f(x)}{∂D(p_{start}^i,p_{start}^t)} * \frac{∂D(p_{start}^i,p_{start}^t)}{∂p_{start}^i} * \frac{∂p_{start}^i}{∂X} ∂X∂f(x)∂D(pstarti,pstartt)∂f(x)∗∂pstarti∂D(pstarti,pstartt)∗∂X∂pstarti
对其中每一项进行计算 ∂ f ( x ) ∂ D ( p s t a r t i , p s t a r t t ) 1 ∂ D ( p s t a r t i , p s t a r t t ) ∂ p s t a r t i ∂ ( p s t a r t i − p s t a r t t ) T ( p s t a r t i − p s t a r t t ) ∂ p s t a r t i p s t a r t i − p s t a r t t D ( p s t a r t i , p s t a r t t ) \frac{∂f(x)}{∂D(p_{start}^i,p_{start}^t)} 1 \\ \frac{∂D(p_{start}^i,p_{start}^t)}{∂p_{start}^i} \frac{∂\sqrt {(p_{start}^i-p_{start}^t)^T (p_{start}^i-p_{start}^t) }}{∂p_{start}^i} \frac{p_{start}^i-p_{start}^t}{D(p_{start}^i,p_{start}^t)} ∂D(pstarti,pstartt)∂f(x)1∂pstarti∂D(pstarti,pstartt)∂pstarti∂(pstarti−pstartt)T(pstarti−pstartt) D(pstarti,pstartt)pstarti−pstartt D对 p s t a r t i p_{start}^i pstarti求导的结果其实就是 进行归一化后的点到直线向量它在xyz三个轴的分量就是前面求解出来的la、lb、lc变量。 ∂ p s t a r t i ∂ X ∂ R c u r r _ s t a r t − 1 ∗ ( p c u r r − 1 − t c u r r _ s t a r t ) ∂ [ r x , r y , r z , t x , t y , t z ] T \frac{∂p_{start}^i}{∂X} \frac{∂ R_{curr\_start}^{-1}*(p_{curr}^{-1}-t_{curr\_start}) }{∂[rx,ry,rz,tx,ty,tz]^T} ∂X∂pstarti∂[rx,ry,rz,tx,ty,tz]T∂Rcurr_start−1∗(pcurr−1−tcurr_start)
用上面 p s t a r t i p_{start}^i pstarti的结果分别对rx,ry,rz,tx,ty,tz求导将得到的结果(3x6矩阵)再与D对 p s t a r t i p_{start}^i pstarti求导的结果(1x3矩阵)相乘就可以得到代码中显示的结果(1x6矩阵)分别赋值到matA的6个位置matB是残差项。
最后使用opencv的QR分解求解增量X即可。 float srx sin(transformTobeMapped[0]);float crx cos(transformTobeMapped[0]);float sry sin(transformTobeMapped[1]);float cry cos(transformTobeMapped[1]);float srz sin(transformTobeMapped[2]);float crz cos(transformTobeMapped[2]);int laserCloudSelNum laserCloudOri-points.size();if (laserCloudSelNum 50) {//如果特征点太少continue;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));//构建雅可比矩阵和残差for (int i 0; i laserCloudSelNum; i) {pointOri laserCloudOri-points[i];coeff coeffSel-points[i];float arx (crx*sry*srz*pointOri.x crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y (crx*cry*srz*pointOri.x crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;float ary ((cry*srx*srz - crz*sry)*pointOri.x (sry*srz cry*crz*srx)*pointOri.y crx*cry*pointOri.z) * coeff.x ((-cry*crz - srx*sry*srz)*pointOri.x (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz ((crz*srx*sry - cry*srz)*pointOri.x (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y ((sry*srz cry*crz*srx)*pointOri.x (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;matA.atfloat(i, 0) arx;matA.atfloat(i, 1) ary;matA.atfloat(i, 2) arz;matA.atfloat(i, 3) coeff.x;matA.atfloat(i, 4) coeff.y;matA.atfloat(i, 5) coeff.z;matB.atfloat(i, 0) -coeff.intensity;}cv::transpose(matA, matAt);matAtA matAt * matA; // H J^T * JmatAtB matAt * matB; // g -J^T * ecv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);7.2 退化处理
代码中使用的退化处理在Ji Zhang的这篇论文(《On Degeneracy of Optimization-based State Estimation Problems》)中有详细论述。
简单来说作者认为退化只可能发生在第一次迭代时先对 H J T ∗ J HJ^T * J HJT∗J矩阵求特征值然后将得到的特征值与阈值(代码中为10)进行比较如果小于阈值则认为该特征值对应的特征向量方向发生了退化将对应的特征向量置为0然后按照论文中所述计算一个P矩阵 P V f − 1 ∗ V u P V_f^{-1} * V_u PVf−1∗Vu V f V_f Vf是原来的特征向量矩阵 V u V_u Vu是将退化方向置为0后的特征向量矩阵然后用P矩阵与原来得到的解相乘得到最终解 X ‘ P ∗ X ∗ X P * X^* X‘P∗X∗ //退化场景判断与处理if (iterCount 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate false;float eignThre[6] {100, 100, 100, 100, 100, 100};for (int i 5; i 0; i--) {if (matE.atfloat(0, i) eignThre[i]) {for (int j 0; j 6; j) {matV2.atfloat(i, j) 0;}isDegenerate true;} else {break;}}matP matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX matP * matX2;}//积累每次的调整量transformTobeMapped[0] matX.atfloat(0, 0);transformTobeMapped[1] matX.atfloat(1, 0);transformTobeMapped[2] matX.atfloat(2, 0);transformTobeMapped[3] matX.atfloat(3, 0);transformTobeMapped[4] matX.atfloat(4, 0);transformTobeMapped[5] matX.atfloat(5, 0);float deltaR sqrt(pow(rad2deg(matX.atfloat(0, 0)), 2) pow(rad2deg(matX.atfloat(1, 0)), 2) pow(rad2deg(matX.atfloat(2, 0)), 2));float deltaT sqrt(pow(matX.atfloat(3, 0) * 100, 2) pow(matX.atfloat(4, 0) * 100, 2) pow(matX.atfloat(5, 0) * 100, 2));//旋转平移量足够小就停止迭代if (deltaR 0.05 deltaT 0.05) {break;}}7.3 更新变换矩阵–transformUpdate()函数
迭代优化完成后使用transformUpdate()函数进行变换矩阵的优化主要是完成对transformBefMapped[6]和transformAftMapped[6]的更新。
如果有IMU的话就使用IMU信息中的roll和pitch角对结果中的变量进行微调微调的原理我也没太看懂感觉就是稍微加了一点点补偿。
然后将transformBefMapped[6]更新为当前帧相对于初始帧的变换 T i n i t _ e n d T_{init\_end} Tinit_end将transformAftMapped[6]更新更新为优化后的当前帧相当于地图坐标系的变换 T m a p _ e n d T_{map\_end} Tmap_end
//优化后进行变换矩阵的更新如果有IMU的则使用IMU进行补偿
void transformUpdate()
{if (imuPointerLast 0) {float imuRollLast 0, imuPitchLast 0;//查找点云时间戳小于imu时间戳的imu位置while (imuPointerFront ! imuPointerLast) {if (timeLaserOdometry scanPeriod imuTime[imuPointerFront]) {break;}imuPointerFront (imuPointerFront 1) % imuQueLength;}if (timeLaserOdometry scanPeriod imuTime[imuPointerFront]) {//未找到,此时imuPointerFrontimuPointerLastimuRollLast imuRoll[imuPointerFront];imuPitchLast imuPitch[imuPointerFront];} else {int imuPointerBack (imuPointerFront imuQueLength - 1) % imuQueLength;float ratioFront (timeLaserOdometry scanPeriod - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);//按时间比例求翻滚角和俯仰角imuRollLast imuRoll[imuPointerFront] * ratioFront imuRoll[imuPointerBack] * ratioBack;imuPitchLast imuPitch[imuPointerFront] * ratioFront imuPitch[imuPointerBack] * ratioBack;}//imu稍微补偿俯仰角和翻滚角transformTobeMapped[0] 0.998 * transformTobeMapped[0] 0.002 * imuPitchLast;transformTobeMapped[2] 0.998 * transformTobeMapped[2] 0.002 * imuRollLast;}//记录优化之前与之后的转移矩阵for (int i 0; i 6; i) {transformBefMapped[i] transformSum[i];transformAftMapped[i] transformTobeMapped[i];}
}八、将当前帧的特征点加入到地图中
下面两个for循环作用是将当前帧的特征点按照cube索引号放入地图中用于之后的匹配然后对周围点云S进行一次降采样更新地图。 /*下面两个for循环作用是将当前帧的特征点放入地图中用于之后的匹配*///将corner points按距离比例尺缩小归入相应的立方体for (int i 0; i laserCloudCornerStackNum; i) {//转移到世界坐标系pointAssociateToMap(laserCloudCornerStack-points[i], pointSel);//按50的比例尺缩小四舍五入偏移laserCloudCen*的量计算索引int cubeI int((pointSel.x 25.0) / 50.0) laserCloudCenWidth;int cubeJ int((pointSel.y 25.0) / 50.0) laserCloudCenHeight;int cubeK int((pointSel.z 25.0) / 50.0) laserCloudCenDepth;if (pointSel.x 25.0 0) cubeI--;if (pointSel.y 25.0 0) cubeJ--;if (pointSel.z 25.0 0) cubeK--;if (cubeI 0 cubeI laserCloudWidth cubeJ 0 cubeJ laserCloudHeight cubeK 0 cubeK laserCloudDepth) {//只挑选-laserCloudCenWidth * 50.0 point.x laserCloudCenWidth * 50.0范围内的点y和z同理//按照尺度放进不同的组每个组的点数量各异int cubeInd cubeI laserCloudWidth * cubeJ laserCloudWidth * laserCloudHeight * cubeK;laserCloudCornerArray[cubeInd]-push_back(pointSel);}}//将surf points按距离比例尺缩小归入相应的立方体for (int i 0; i laserCloudSurfStackNum; i) {pointAssociateToMap(laserCloudSurfStack-points[i], pointSel);int cubeI int((pointSel.x 25.0) / 50.0) laserCloudCenWidth;int cubeJ int((pointSel.y 25.0) / 50.0) laserCloudCenHeight;int cubeK int((pointSel.z 25.0) / 50.0) laserCloudCenDepth;if (pointSel.x 25.0 0) cubeI--;if (pointSel.y 25.0 0) cubeJ--;if (pointSel.z 25.0 0) cubeK--;if (cubeI 0 cubeI laserCloudWidth cubeJ 0 cubeJ laserCloudHeight cubeK 0 cubeK laserCloudDepth) {int cubeInd cubeI laserCloudWidth * cubeJ laserCloudWidth * laserCloudHeight * cubeK;laserCloudSurfArray[cubeInd]-push_back(pointSel);}}//因为加入了当前sweep的点所以这个for循环对局部点云集合S进行降采样然后更新总cube集合for (int i 0; i laserCloudValidNum; i) {int ind laserCloudValidInd[i]; //局部点云集合S的cube序号laserCloudCornerArray2[ind]-clear();downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);//滤波输出到Array2laserCloudSurfArray2[ind]-clear();downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);//Array与Array2交换即滤波后自我更新pcl::PointCloudPointType::Ptr laserCloudTemp laserCloudCornerArray[ind];laserCloudCornerArray[ind] laserCloudCornerArray2[ind];laserCloudCornerArray2[ind] laserCloudTemp;//laserCloudCornerArray更新成降采样之后的了laserCloudTemp laserCloudSurfArray[ind];laserCloudSurfArray[ind] laserCloudSurfArray2[ind];laserCloudSurfArray2[ind] laserCloudTemp;}
九、话题发布
最后就是一些发提的发布包括以下话题
/laser_cloud_surround每隔5帧发布一次(这种一般都是用于可视化的情况)降采样后的周围点云(不只是视域范围内的而且周围的全部发布)/velodyne_cloud_registered变换到地图坐标系下的全部点云/aft_mapped_to_init经过laserMapping优化后的当前帧到初始时刻(或者到地图map坐标系)的坐标变换
并且广播了/camera_init到/aft_mapped的坐标变换 mapFrameCount;//特征点汇总降采样每隔五帧publish一次从第一次开始if (mapFrameCount mapFrameNum) {// 每隔5帧发布S集合mapFrameCount 0;laserCloudSurround2-clear();for (int i 0; i laserCloudSurroundNum; i) {int ind laserCloudSurroundInd[i]; // 局部地图所有cube序号*laserCloudSurround2 *laserCloudCornerArray[ind];*laserCloudSurround2 *laserCloudSurfArray[ind];}laserCloudSurround-clear();downSizeFilterCorner.setInputCloud(laserCloudSurround2);downSizeFilterCorner.filter(*laserCloudSurround);sensor_msgs::PointCloud2 laserCloudSurround3;pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);laserCloudSurround3.header.stamp ros::Time().fromSec(timeLaserOdometry);laserCloudSurround3.header.frame_id /camera_init;pubLaserCloudSurround.publish(laserCloudSurround3);}//将点云中全部点转移到世界坐标系下int laserCloudFullResNum laserCloudFullRes-points.size();for (int i 0; i laserCloudFullResNum; i) {pointAssociateToMap(laserCloudFullRes-points[i], laserCloudFullRes-points[i]);}sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp ros::Time().fromSec(timeLaserOdometry);laserCloudFullRes3.header.frame_id /camera_init;pubLaserCloudFullRes.publish(laserCloudFullRes3);geometry_msgs::Quaternion geoQuat tf::createQuaternionMsgFromRollPitchYaw(transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);odomAftMapped.header.stamp ros::Time().fromSec(timeLaserOdometry);odomAftMapped.pose.pose.orientation.x -geoQuat.y;odomAftMapped.pose.pose.orientation.y -geoQuat.z;odomAftMapped.pose.pose.orientation.z geoQuat.x;odomAftMapped.pose.pose.orientation.w geoQuat.w;odomAftMapped.pose.pose.position.x transformAftMapped[3];odomAftMapped.pose.pose.position.y transformAftMapped[4];odomAftMapped.pose.pose.position.z transformAftMapped[5];//扭转量odomAftMapped.twist.twist.angular.x transformBefMapped[0];odomAftMapped.twist.twist.angular.y transformBefMapped[1];odomAftMapped.twist.twist.angular.z transformBefMapped[2];odomAftMapped.twist.twist.linear.x transformBefMapped[3];odomAftMapped.twist.twist.linear.y transformBefMapped[4];odomAftMapped.twist.twist.linear.z transformBefMapped[5];pubOdomAftMapped.publish(odomAftMapped);//广播坐标系旋转平移参量aftMappedTrans.stamp_ ros::Time().fromSec(timeLaserOdometry);aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], transformAftMapped[4], transformAftMapped[5]));tfBroadcaster.sendTransform(aftMappedTrans);}}status ros::ok();rate.sleep();}return 0;
}总结
我个人感觉laserMapping这个模块是比laserOdometry模块简单很多的主要是不涉及到IMU就省很多事我觉得这部分需要仔细想一想的地方就是地图维护更新的那一部分中间优化部分很简单laserOdometry中的那部分弄懂了这部分就很容易看明白了。
论文中说的这部分是低频率高精度按照这个代码它的频率是5Hz但是只要改变一下控制跳帧变量就很容易得到文章中的1Hz高精度我觉得主要是体现在它使用的是scan-to-map的方式进行匹配用于匹配的数据量很大从而提高了匹配精度因为它的迭代次数是比laserOdometry少的。
下一篇将介绍最后一个模块transformMaintenance.cpp也就是laserOdometry和laserMapiing求解到的变量进行融合得到最终变换的模块。
