【3D激光SLAM】LOAM源代码解析--laserMapping.cpp

系列文章目录

·【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的匹配过程,它的具体功能如下:

  1. 接收特征点话题、全部点云话题、IMU话题,并保存到对应的变量中
  2. 将当前sweep与上几次sweep(局部地图)进行特征匹配,得到edge point在局部子图中KD-tree查找到的5个最近点以及planar point在局部子图KD-tree查找到的5个最近点
  3. 计算edge point对应5个点构成的均值和协方差矩阵,对协方差矩阵进行分解,求解匹配到的直线参数
  4. 用planar point对应5个点构建一个超定方程组,求解得到平面参数
  5. 构建雅可比矩阵和残差项,求解优化后的位姿变换
  6. 将当前帧的点云插入局部子图,维护一个局部地图,用于下一帧的匹配
  7. 发布话题并更新tf变换

一、变量含义

首先,介绍一下本程序用到变量的含义,理解这一部分非常重要:

  • transformBefMapped[6]:经过laserMapping模块优化前的,上一帧相对于初始时刻的位姿变换 T i n i t _ s t a r t T_{init\_start} Tinit_start
  • transformSum[6]:从laserOdometry模块接收到的,当前帧相对于初始时刻的变换 T i n i t _ s t a r t T_{init\_start} Tinit_start
  • transformAftMapped[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_start
  • transformTobeMapped[6]:在laserMapping模块中用于优化的变量,优化完成后会赋值给transformAftMapped[6]

一些理解:虽然transformAftMapped[6]我上面写的是 T m a p _ e n d T_{map\_end} Tmap_end,看起来好像是把坐标系换成了map坐标系,但是我觉得这里有两种理解都可以:

  1. AftMapped可以理解为经过laserMapping模块优化后的里程计坐标系下的当前帧end相对于初始帧的坐标变换
  2. 也可以理解为经过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.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2, laserCloudFullResHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);ros::Publisher pubOdomAftMapped = nh.advertise<nav_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::vector<int> pointSearchInd;std::vector<float> 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::VoxelGrid<PointType> downSizeFilterCorner;//设置体素大小downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);pcl::VoxelGrid<PointType> downSizeFilterSurf;downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);pcl::VoxelGrid<PointType> downSizeFilterMap;downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);//指针初始化for (int i = 0; i < laserCloudNum; i++) {laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());laserCloudCornerArray2[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray2[i].reset(new pcl::PointCloud<PointType>());}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} tinitstartend

然后将其变换到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} tinitstartend=Rend_inittinitstartend=Rinit_end1tinitstartendRinit_end1=RZXY1=RrzRrxRry
对应于下面代码中所示的变换。

2.求解旋转部分的初始猜测
现在这里的变量含义分别表示为:

  • transformSum:当前帧相对于初始帧的变换 R i n i t _ e n d R_{init\_end} Rinit_end
  • transformBefMapped:上一帧相对于初始帧的变换 R i n i t _ s t a r t R_{init\_start} Rinit_start
  • transformAftMapped:上一帧优化后的相对于初始帧的变换,也可以理解为上一帧相对于地图坐标系的变换 R m a p _ s t a r t R_{map\_start} Rmap_start
  • transformTobeMapped:当前帧相对于初始帧的初始猜测,也可以理解为当前帧相对于地图坐标系的变换 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_end=Rmap_startRinit_start1Rinit_end=RZXYRZXY1RZXY

这里的计算公式与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} cacycacz+sacxsacysacz& cacysacz+sacxsacycacz& cacxsacy\\ cacxsacz& cacxcacz& -sacx\\ -sacycacz+sacxcacysacz& sacysacz+sacxcacycacz& cacxcacy\\ \end{matrix} \right] R~map_end= cacycacz+sacxsacysaczcacxsaczsacycacz+sacxcacysaczcacysacz+sacxsacycaczcacxcaczsacysacz+sacxcacycaczcacxsacysacxcacxcacy
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} cbcycbcz+sbcxsbcysbcz& cbcysbcz+sbcxsbcycbcz& cbcxsbcy\\ cbcxsbcz& cbcxcbcz& -sbcx\\ -sbcycbcz+sbcxcbcysbcz& sbcysbcz+sbcxcbcycbcz& cbcxcbcy\\ \end{matrix} \right] Rmap_start= cbcycbcz+sbcxsbcysbczcbcxsbczsbcycbcz+sbcxcbcysbczcbcysbcz+sbcxsbcycbczcbcxcbczsbcysbcz+sbcxcbcycbczcbcxsbcysbcxcbcxcbcy
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& sblycblz+sblxcblysblz\\ -cblysblz+sblxsblycblz& cblxcblz& sblysblz-sblxcblycblz\\ -cblxsbly& sblx& cblxcbly\\ \end{matrix} \right] Rinit_start1= cblycblzsblxsblysblzcblysblz+sblxsblycblzcblxsblycblxsblzcblxcblzsblxsblycblz+sblxcblysblzsblysblzsblxcblycblzcblxcbly
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} calycalz+salxsalysalz& calysalz+salxsalycalz& calxsaly\\ calxsalz& calxcalz& -salx\\ -salycalz+salxcalysalz& salysalz+salxcalycalz& calxcaly\\ \end{matrix} \right] Rinit_end= calycalz+salxsalysalzcalxsalzsalycalz+salxcalysalzcalysalz+salxsalycalzcalxcalzsalysalz+salxcalycalzcalxsalysalxcalxcaly

然后使用对应位置的值相等,就得到了修正后的累计变换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(salxsblx+calxcalycblxcbly+calxcblxsalysbly)cbcxcbcz(calxsaly(cblysblzcblzsblxsbly)calxcaly(sblysblz+cblycblzsblx)+cblxcblzsalx)cbcxsbcz(calxcaly(cblzsblycblysblxsblz)calxsaly(cblycblz+sblxsblysblz)+cblxsalxsblz))acy=arctan(R1,3/R3,3)acz=arctan(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 tmapincrement=R~map_endtendincrementR~map_end=RZXY=RyRxRz

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_end=tmap_start+tmapendstart=tmap_starttmapstartend

//基于匀速模型,根据上次微调的结果和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} pmap=R~map_endpend+tmap_end=RyRxRzpend+tmap_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的总集合,一共有laserCloudNum=211121个cube,一个cube尺寸为50x50x50米
  • laserCloudSurfArray:存放planar point的总地图,按照论文的话就是所有cube的总集合,一共有laserCloudNum=211121个cube,一个cube尺寸为50x50x50米
  • laserCloudWidth:宽度方向cube数量//21
  • laserCloudHeight:高度方向cube数量//11
  • laserCloudDepth:深度方向cube数量//21
  • laserCloudNum:总cube数量 = laserCloudWidth * laserCloudHeight * laserCloudDepth;//21x11x21
  • laserCloudCenWidth:地图中心点在宽度方向的偏移量//(21+1)/2
  • laserCloudCenHeight:地图中心点在高度方向的偏移量//(11+1)/2
  • laserCloudCenDepth:地图中心点在深度方向的偏移量//(21+1)/2
  • laserCloudSurroundInd:构成周围点云cube集合的索引,对应论文中构成S`的cube的索引
  • laserCloudValidInd:构成周围点云cube集合,并且在激光雷达视域范围内的索引
  • centerCubeI:机器人位置在地图中宽度方向的cube索引
  • centerCubeJ:机器人位置在地图中高度方向的cube索引
  • centerCubeK:机器人位置在地图中深度方向的cube索引

下面就是求解机器人位置的cube索引centerCubeI、centerCubeJ、centerCubeK,下图源自:SLAM前端入门框架-A_LOAM源码解析-知乎在这里插入图片描述
然后下面六个while循环,就是循环移位的过程,如果机器人的位置靠近了地图边缘,那就调整地图的中心靠近机器人的位置,比如第一个while循环,意思是如果centerCubeI<3,说明机器人位置处于下边界,表明地图向负方向延伸的可能性比较大,则循环移位,将数组中心点向上边界调整一个单位。

调整后需要满足:

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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//that's [i + 21 * j + 231 * k]pcl::PointCloud<PointType>::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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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。

筛选方法如下:

  1. 先将周围125个cube的每一个cube索引换算成在世界坐标系下的坐标(以米为单位)
  2. 取周围125个cube的每一个cube的上下左右八个顶点坐标
  3. 求原点到顶点距离的平方和机器人上方10m一个点pointOnYAxis到顶点距离的平方
  4. 应用余弦定理求解角度,如果在(30,150)范围内则认为在视域内,图示如下

在这里插入图片描述
如果check1<0并且check2>0,就说明图示中的θ角在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//如果check1<0说明:这个夹角大于30度,在视野范围内float check1 = 100.0 + squaredSide1 - squaredSide2- 10.0 * sqrt(3.0) * sqrt(squaredSide1);//check2其实是:机器人原点位置与cube中点的连线 与 机器人上方10m线段 的夹角 的余弦 - cos150//如果check2>0说明:这个夹角小于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.at<float>(0, 0) = a11;matA1.at<float>(0, 1) = a12;matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12;matA1.at<float>(1, 1) = a22;matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13;matA1.at<float>(2, 1) = a23;matA1.at<float>(2, 2) = a33;//特征值分解cv::eigen(matA1, matD1, matV1);// 论文中:最大特征值大于次大特征值的3倍认为是线特征if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {//如果最大的特征值大于第二大的特征值三倍以上float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = cx + 0.1 * matV1.at<float>(0, 0);float y1 = cy + 0.1 * matV1.at<float>(0, 1);float z1 = cz + 0.1 * matV1.at<float>(0, 2);float x2 = cx - 0.1 * matV1.at<float>(0, 0);float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(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.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;}//求解matA0*matX0=matB0cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);float pa = matX0.at<float>(0, 0);float pb = matX0.at<float>(1, 0);float pc = matX0.at<float>(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、tz;D()表示两点之间的距离,其计算公式为:
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)=(pstartipstartt)T(pstartipstartt)

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& -crysrz+srxsrycrz& -crxsry\\ -crxsrz& crxcrz& srx\\ srycrz+srxcrysrz& srysrz-srxcrycrz& crxcry\\ \end{matrix} \right] \cdot \left[ \begin{array}{c} p_{curr}-tx\\ p_{curr}-ty\\ p_{curr}-tz\\ \end{array} \right] pstarti=Rcurr_start1(pcurr1tcurr_start)= crycrzsrxsrysrzcrxsrzsrycrz+srxcrysrzcrysrz+srxsrycrzcrxcrzsrysrzsrxcrycrzcrxsrysrxcrxcry pcurrtxpcurrtypcurrtz

根据链式法则,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} Xf(x)=D(pstarti,pstartt)f(x)pstartiD(pstarti,pstartt)Xpstarti

对其中每一项进行计算:
∂ 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)=1pstartiD(pstarti,pstartt)=pstarti(pstartipstartt)T(pstartipstartt) =D(pstarti,pstartt)pstartipstartt
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} Xpstarti=[rx,ry,rz,tx,ty,tz]TRcurr_start1(pcurr1tcurr_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.at<float>(i, 0) = arx;matA.at<float>(i, 1) = ary;matA.at<float>(i, 2) = arz;matA.at<float>(i, 3) = coeff.x;matA.at<float>(i, 4) = coeff.y;matA.at<float>(i, 5) = coeff.z;matB.at<float>(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 H=J^T * J H=JTJ矩阵求特征值,然后将得到的特征值与阈值(代码中为10)进行比较,如果小于阈值则认为该特征值对应的特征向量方向发生了退化,将对应的特征向量置为0,然后按照论文中所述,计算一个P矩阵:
P = V f − 1 ∗ V u P = V_f^{-1} * V_u P=Vf1Vu

V f V_f Vf是原来的特征向量矩阵, V u V_u Vu是将退化方向置为0后的特征向量矩阵,然后用P矩阵与原来得到的解相乘,得到最终解:

X ‘ = P ∗ X ∗ X` = P * X^* X=PX

            //退化场景判断与处理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.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(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.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +pow(rad2deg(matX.at<float>(1, 0)), 2) +pow(rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(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]) {//未找到,此时imuPointerFront==imuPointerLastimuRollLast = 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::PointCloud<PointType>::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求解到的变量进行融合得到最终变换的模块。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/53974.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【Jenkins】持续集成部署学习

【Jenkins】持续集成部署学习 【一】Jenkins介绍【二】Docker安装Gitlab【1】首先准备一台空的虚拟机服务器【2】安装服务器所需的依赖【3】Docker的安装【4】阿里云镜像加速【5】安装Gitlab 【三】Gitlab的使用&#xff08;1&#xff09;Gitlab创建项目&#xff08;2&#xff…

SpringBoot案例-配置文件-参数配置化

前言 目前我们已经完成了部门管理和员工管理功能接口的实现&#xff0c;阿里云OSS工具类中&#xff0c;我们会设置4个参数&#xff0c;分别是云服务域名、云服务ID和密码、文件存储的Bucket、就会存在以下问题&#xff1a;参数配置分散以及参数发生变化&#xff0c;就需要对应…

数据结构—循环队列(环形队列)

循环队列&#xff08;环形队列&#xff09; 循环队列的概念及结构循环队列的实现 循环队列的概念及结构 循环队列是一种线性数据结构&#xff0c;其操作表现基于 FIFO&#xff08;先进先出&#xff09;原则并且队尾被连接在队首之后以形成一个循环。它也被称为“环形缓冲器”。…

同为科技(TOWE)带热插拔功能机柜PDU插座的应用

所谓热插拔&#xff08;hot-plugging或Hot Swap&#xff09;&#xff0c;即带电插拔&#xff0c;指的是在不关闭系统电源的情况下&#xff0c;将模块、板卡插入或拔出系统而不影响系统的正常工作&#xff0c;从而提高了系统的可靠性、快速维修性、冗余性和对灾难的及时恢复能力…

HVV爆火漏洞:最新 WPS RCE (远程命令执行) 复现

最近HVV爆出的很火的WPS命令执行漏洞&#xff0c;其实并不是0DAY&#xff0c;早在2019年就出现了&#xff0c;只不过最近EXP才公开。接下来我们来复现一遍。 0x00 影响版本 WPS Office 2023 个人版 < 11.1.0.15120WPS Office 2019 企业版 < 11.8.2.12085 0x01 环境配置…

[管理与领导-50]:IT基层管理者 - 8项核心技能 - 5 - 沟通是润滑剂

目录 前言&#xff1a; 一、什么是沟通 1.1 定义 1.2 沟通模型 1.3 沟通的六层次模型 1.4 为什么需要沟通 二、沟通的五维度 三、沟通的原则 3.1 以终为始 3.2 双赢思维&#xff1a;人们只会做对自己有利的事 3.3 牵善的思维 四、沟通的过程 五、沟通技巧 六、深…

【HSPCIE仿真】输入网表文件(1)基本内容和基本规则

输入网表文件 1. 输入网表文件基本内容2. 输入网表文件示例3. 一些基本规则4. 数值表示5. 压缩文件格式的读取6. 参数和表达式 从HSPICE的仿真流程看&#xff0c;出去初始化配置过程&#xff0c;真正的仿真是从输入网表文件开始的。 HSPICE 根据输入网表文件&#xff08; inpu…

【80天学习完《深入理解计算机系统》】第十一天 3.4 跳转指令

专注 效率 记忆 预习 笔记 复习 做题 欢迎观看我的博客&#xff0c;如有问题交流&#xff0c;欢迎评论区留言&#xff0c;一定尽快回复&#xff01;&#xff08;大家可以去看我的专栏&#xff0c;是所有文章的目录&#xff09;   文章字体风格&#xff1a; 红色文字表示&#…

深度学习7:生成对抗网络 – Generative Adversarial Networks | GAN

生成对抗网络 – GAN 是最近2年很热门的一种无监督算法&#xff0c;他能生成出非常逼真的照片&#xff0c;图像甚至视频。我们手机里的照片处理软件中就会使用到它。 目录 生成对抗网络 GAN 的基本原理 大白话版本 非大白话版本 第一阶段&#xff1a;固定「判别器D」&#x…

设计模式-适配器模式

核心思想 见名知意&#xff0c;是作为两个不兼容的接口的桥梁&#xff0c;属于结构型模式使得原来由于接口不兼容而不能一起工作的那些类可以一起工作 常见的几类适配器 类的适配器模式 想将一个类转换成满足另外一个新接口的类时&#xff0c;可以使用类的适配器模式&#x…

基于CH552G主控的开源九键小键盘(资料齐全)

Content 1. 前言2. CH55X Arduino平台环境搭建2.1 常规安装2.2 HFS挂载本地服务器安装 3. 例程使用3.1 工程下载及编译3.2 程序烧录 1. 前言 芯片选型&#xff1a;CH552G 本文主要解决Arduino下CH552G芯片包的环境配置问题 开源地址&#xff1a;CV键盘有线小键盘 - 嘉立创ED…

一分钟学会用pygame制作棋盘背景

一分钟一个Pygame案例&#xff0c;这一集我们来学习一下如何生成一个视频中的棋盘背景效果&#xff0c;非常非常简单。 视频教程链接&#xff1a;https://www.bilibili.com/video/BV17G411d7Ah/ 当然我们这里是用来做页面的背景&#xff0c;你也可以拿来做别的效果&#xff0…

fdm-cli,一个致力于管理项目初始化模板的工具脚手架

希望各位可以了解fdm-cli&#xff0c;并在合适的时候尝试使用一下。 阅读本文你将获得 一个&#xff08;好用的&#xff09;项目模板初始化工具这个&#xff08;好用的&#xff09;工具的使用方法给作者点一个 star 的机会 一、不愿再复制粘贴 小N每次在写项目的时候&#x…

JVM及垃圾回收机制

文章目录 1、JVM组成&#xff1f;各部分作用&#xff1f;1.1 类加载器&#xff08;Class Loaders&#xff09;1.2 运行时数据区&#xff08;Runtime Data Area&#xff09;1.3 执行引擎&#xff08;Execution Engine&#xff09;1.4 本地方法接口&#xff08;Native Interface&…

按钮权限控制

搜索关键字&#xff1a; 自定义指令传参| "自定义指令""dataset"|自定义指令dataset| "Vue""directives"|vue按钮权限实现 1、完整代码&#xff1a; <template> <div> <el-breadcrumb separator-class"el-icon…

淘宝免费爬虫数据 商品详情数据 商品销售额销量API

场景&#xff1a;一个宽敞明亮的办公室&#xff0c;一位公司高管坐在办公桌前。 高管&#xff08;自言自语&#xff09;&#xff1a;淘宝&#xff0c;这个平台上商品真是琳琅满目&#xff0c;应该有不少销售数据吧。我该怎么利用这些数据呢&#xff1f; 突然&#xff0c;房间…

Oracle跨库访问DBLINK

1. DBLINK的介绍 Oracle在进行跨库访问时&#xff0c;可以创建DBLINK实现&#xff0c;比如要将UAT的表数据灌入开发环境&#xff0c;则可以使用UAT库为数据源&#xff0c;通过DBLINK实现将查出的数据灌入开发库。 简而言之就是在当前数据库中访问另一个数据库中的表中的数据 2…

js中作用域的理解?

1.作用域 作用域&#xff0c;即变量(变量作用域又称上下文)和函数生效(能被访问)的区域或集合 换句话说&#xff0c;作用域决定了代码区块中变量和其他资源的可见性 举个例子 function myFunction() {let inVariable "函数内部变量"; } myFunction();//要先执行这…

网络安全在医疗行业中的重要性

不可否认&#xff0c;现代世界见证了技术和医疗行业的交织&#xff0c;塑造了我们诊断、治疗和管理健康状况的新方式。随着电子健康记录取代纸质文件&#xff0c;远程医疗缩短了患者和医疗服务提供者之间的距离&#xff0c;数字化转型既是福音&#xff0c;也是挑战。最近的全球…

Hightopo 使用心得(6)- 3D场景环境配置(天空球,雾化,辉光,景深)

在前一篇文章《Hightopo 使用心得&#xff08;5&#xff09;- 动画的实现》中&#xff0c;我们将一个直升机模型放到了3D场景中。同时&#xff0c;还利用动画实现了让该直升机围绕山体巡逻。在这篇文章中&#xff0c;我们将对上一篇的场景进行一些环境上的丰富与美化。让场景更…