LOAM: LiDAR Odometry and Mapping In Real Time源码解析(三)

最新推荐文章于 2023-03-23 22:20:49 发布

魏新slam

最新推荐文章于 2023-03-23 22:20:49 发布

阅读量1.6k

点赞数 5

分类专栏： 3D激光SLAM源码解析系列文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_17693963/article/details/107519669

版权

3D激光SLAM源码解析系列专栏收录该内容

14 篇文章 50 订阅

订阅专栏

LOAM：LiDAR Odometry and Mapping In Real Time源码解析（三）

LiDAR Mapping
总结

LiDAR Mapping

上一篇博客中介绍了LOAM中的LiDAR Odometry部分，本篇博客将主要介绍LOAM中的LiDAR Mapping部分。

输入

首先我们来看LiDAR Mapping部分的输入，从laserMapping.cpp文件中的主函数可以看出，LiDAR Mapping部分的输入为：corner特征，surface特征，激光里程计结果以及原始激光雷达点云。其中从订阅的消息名称可以看出，corner特征对应的是LiDAR Odometry部分发布的当前帧的less sharp corner，surface特征对应的是LiDAR Odometry部分发布的当前帧的less flat surface。

ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);

特征地图维护

经过简单的信息同步后，如下代码所示，LOAM首先利用激光里程计的结果更新了Mapping部分维护的激光里程计计算出的当前帧在odom坐标系下的位姿q_wodom_curr及t_wodom_curr。

q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();

然后利用上一帧LiDAR Mapping计算出的odom坐标系到map坐标系的变换关系，得到当前帧在map坐标系下的位姿的预测，即 $T^{curr}_{w}=T^{odom}_{map}T^{curr}_{odom}$ 。

transformAssociateToMap();

void transformAssociateToMap()
{
    q_w_curr = q_wmap_wodom * q_wodom_curr;
    t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}

然后根据预测位姿，可以得到当前帧在3D栅格特征地图中的位置。

// 计算当前预测位置在3D栅格中的位置            
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;                        
if (t_w_curr.x() + 25.0 < 0)                
    centerCubeI--;            
if (t_w_curr.y() + 25.0 < 0)                
    centerCubeJ--;            
if (t_w_curr.z() + 25.0 < 0)                
    centerCubeK--;

为说明LOAM中维护特征地图的方式，首先简单介绍一下LOAM中特征地图的组织形式。LOAM中的特征地图是由corner和surface特征点构成的，并将这些特征点划分到了不同的3D栅格中。每个3D栅格对应的物理尺寸为50m×50m×50m，x方向(宽)共21行栅格，y方向(高)共21行栅格，z方向(深度)共11层，因此3D栅格特征地图中共有21×21×11=4851个栅格。3D栅格地图中心对应的x方向索引laserCloudCenWidth为10，y方向索引laserCloudCenHeight为10，laserCloudCenDepth为5。初始时刻，即t_w_curr.x=t_w_curr.y=t_w_curr.z=0时，激光雷达位于3D栅格的中心处。然后就可以通过栅格索引的方式，搜索附近的特征点云。一切看起来都很美好，但是有个问题啊，假如激光雷达一直沿着x方向前进，那总有激光雷达运动范围超过界限的时刻，毕竟这个3D栅格地图在x方向表示的运动范围最大为21×50m=1050m。因此这就是LOAM中需要维护特征地图的原因了。
从下示代码中的第一个while循环中可以看出，当centerCubeI<3时，即当激光雷达朝着x轴负方向运动且将要达到特征地图x负方向边界时，LOAM将特征地图依次向x轴负方向移动一个栅格；同理，从第二个while循环中可以看出，当centerCubeI>=laserCloudWidth-3时，即当激光雷达朝着x轴正方向运动且将要到达特征地图x正方向边界时，LOAM将特征地图依次向x轴正方向移动一个栅格。对y方向和z方向栅格的维护也采用了这样的策略，不再赘述。这样做的好处在于尽量将激光雷达保持在特征地图中心处以保证在做点云配准时可以保证在激光雷达附近可以找到特征点云地图中的特征。

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];
            pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer = laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];                            
            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){                 
    for (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];
            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--;            
}

维护过特征地图后，即可根据3D栅格地图的索引，得到当前激光雷达附近的特征点：

for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++{
    for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++){                    
        for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++){
            if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth){
                laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
                laserCloudValidNum++;
                laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
                laserCloudSurroundNum++;
            }
        }
    }
}                                                                                                                                                                                                                                               
laserCloudCornerFromMap->clear();            
laserCloudSurfFromMap->clear();            
for (int i = 0; i < laserCloudValidNum; i++){                
    *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
    *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

位姿优化

得到当前帧的特征点 $F_{curr}$ 以及附近特征点地图中的特征点 $F_{map}$ 后，以 $F_{curr}$ 为source点云，以 $F_{map}$ 为target点云，即可利用和LiDAR Odometry中类似的点到线和点到面距离构成的残差函数，优化当前帧在世界坐标系或地图坐标系下的位姿 $T^{curr}_w$ 。整个过程与LiDAR Odometry中的类似，不再赘述。不过需要指出的是，在LiDAR Mapping过程中，LOAM采用了特征值分析的方法来校验点到线和点到面的关联。

更新位姿和地图

得到优化后的位姿 $T^{curr}_w$ 后，需要首先更新odom坐标系到map坐标系的变换关系 $T^{odom}_{map}$ ，即 $T^{odom}_{map}=T^{curr}_{w}(T^{curr}_{odom})^{-1}$ 如在transformUpdate函数中所示。

void transformUpdate()
{
    q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
    t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}

更新过 $T^{odom}_{map}$ 后，需要更新特征地图，这部分比较简单，即将当前帧的特征点投影到地图坐标系后，再映射到不同的3D栅格，并将该特征点添加到该栅格内。

for (int i = 0; i < laserCloudCornerStackNum; i++){
    pointAssociateToMap(&laserCloudCornerStack->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;                    
    laserCloudCornerArray[cubeInd]->push_back(pointSel);                
    }            
}

总结

LOAM提出了一种新颖的激光雷达稀疏点云特征提取方法，计算高效，且通过在每一个scanline中分别提取corner和surface特征，避免了旋转式激光雷达垂直角分辨率较大带来的对特征提取的影响。
构建了一种基于点到线距离以及点到面距离的激光里程计，且考虑了激光雷达连续测量导致的畸变问题
特征提取的不稳定性以及稀疏性，导致激光里程计位姿估计不准确。为提高里程计整体精度，提出了一种Mapping的策略，即维护稠密特征地图，利用单帧特征点(less sharp, less flat)与稠密特征地图配准的方式，周期性地修正激光里程计误差。
很多人喜欢说LOAM最大的缺点是缺少回环检测功能，我刚开始看LOAM源码的时候也觉得这是个比较重要的缺陷，但现在觉得LOAM通过维护稠密特征地图以及Mapping过程，也是可以实现全局地图一致性的，只是没有显式地进行回环检测而已，所以个人觉得这并不是LOAM的什么缺陷。
最近时间比较紧迫，有纰漏之处欢迎指正。