admin管理员组

文章数量:1622625

作者丨王方浩@知乎

来源丨https://zhuanlan.zhihu/p/382460472

编辑丨3D视觉工坊

LeGO-LOAM是一种激光雷达SLAM算法,对应的论文为《LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain》,同时有开源代码[1]。

下面我们将结合论文和代码对LeGO-LOAM做一个简单分析,LeGO-LOAM一共分为4个流程,代码结构上也大致按照这4个流程进行划分,因此接下来也会分4个章节来介绍LeGO-LOAM的算法流程。本章首先介绍LeGO-LOAM的主流程,然后重点介绍点云分割的流程。

LeGO-LOAM介绍

一开始以为LeGO-LOAM的LeGO是代表乐高积木,可以类似乐高积木的方式来搭建SLAM算法,直到看到论文才搞清楚,LeGO的Le表示轻量级(Lightweight),GO表示基于地面优化(Ground-Optimized)。也就是说LeGO-LOAM一是一个轻量级,基于地面优化的激光雷达SLAM算法。

LeGO-LOAM采用的硬件平台是Jackal UGV,整体的系统架构如下图。

LeGO-LOAM一共分为4个步骤。

1.Segmentation (filter out noise)

2.Feature Extraction(distinctive planar and edge features)

3.Lidar Odometry :Label Matching, Two-step L-M Optimization

4.Lidar Mapping(pose-graph SLAM)

首先是对输入的原始点云进行点云分割(Segmentation),找到地面并且进行点云分割,接着对分割好的点云进行特征提取(Feature Extraction),找到面特征和边特征,提取出特征之后接下来进行特征匹配,并且输出位姿,最后对点云进行注册,生成全局地图,并且进行回环检测,对生成的地图进行优化。

接下来我们开始介绍点云分割。

点云分割

点云分割主要的流程是先进行地面提取,然后对剩下的点云进行分割,最后拿分割好的点云进行到下一步的特征提取。

由于机器人可能在复杂的环境中,小物体(例如树叶)可能会形成微不足道且不可靠的特征,因为在两次连续扫描中不太可能看到相同的树叶。为了提高效率和可靠性,分割的时候忽略了少于30个点的集群(实际上代码中也保留了5个以上点,并且横向线数大于3的集群)。

在这个过程之后,只保留可能代表大物体的点,例如树干和地面点以供进一步处理。下图中(a)是原始点云,(b)是分割后的点云,红色代表地面,剩余点是分割后的点云,可以看到去除了大量的噪音。

代码分析

点云分割对应的代码在"LeGO-LOAM/src/imageProjection.cpp"中,LeGO-LOAM的4个流程都是独立运行的ROS程序,入口函数如下

int main(int argc, char** argv){

    ros::init(argc, argv, "lego_loam");
    
    ImageProjection IP;   // 工作主流程

    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");

    ros::spin();
    return 0;
}

主要的工作流程实际上在"ImageProjection IP"中,那么为什么没有任何函数执行就可以工作呢?实际上在ImageProjection类的构造函数中,创建了订阅消息和回调,这样只要ImageProjection类IP一创建就会执行构造函数,也就开启了发布订阅流程,在点云消息到来之后,就会触发点云处理回调。

我们接着看构造函数

    ImageProjection():
        nh("~"){
// 1. 点云回调,主要的工作流程
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);

// 2. 以下为发布地面点云和分割好的点云
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);

        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;

// 3. 申请资源
        allocateMemory();
// 4. 初始化
        resetParameters();
    }

源码中在构造函数申请了资源,有些是智能指针,会自动释放资源,但是另外有4个数组不会,会产生资源泄露。最好在析构函数中进行资源释放。

直到这里才找到真正的点云处理函数,也就是点云的回调函数"cloudHandler"。cloudHandler的流程非常清晰,分为7步。

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        // 1. 将 ros 消息转换为 pcl 点云
        copyPointCloud(laserCloudMsg);
        // 2. 扫描的开始和结束角度
        findStartEndAngle();
        // 3. 距离图像投影
        projectPointCloud();
        // 4. 标记地面点云
        groundRemoval();
        // 5. 点云分割
        cloudSegmentation();
        // 6. 发布分割后的点云
        publishCloud();
        // 7. 为下一次迭代重置参数
        resetParameters();
    }

接下来我们逐步分析这7个过程,其中主要的过程是“3. 距离图像投影、4. 标记地面点云和5. 点云分割”

1. copyPointCloud

将 ros 消息转换为 pcl 点云,这里对velodyne的lidar做了区分处理,以16线雷达为例,激光雷达通过16根雷达光束进行旋转扫描,从而得到一周360°的点云。velodyne的lidar对点云属于16线中的哪一线做了标记,这个标记被称为Ring index,其中的Ring表示环的意思。如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束。因此保存点云的时候分别保存为"laserCloudIn"和"laserCloudInRing"中。

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        // 1. 读取ROS点云转换为PCL点云
        cloudHeader = laserCloudMsg->header;
        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
        // 2. 去除点云中的Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
        // 3. 如果点云有"ring"通过,则保存为laserCloudInRing
        if (useCloudRing == true){
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false) {
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }  
        }
    }

2. findStartEndAngle*

查找起始角度和终止角度,这里的角度是激光雷达旋转一周的起始角度和终止角度,选取的起点是points[0],选取的终点是points[laserCloudIn->points.size() - 1],是否是说点云是按照顺序排列的呢?另外这里最后一个点并不一定是最大角度。

此外还对x,y进行了坐标转换,关于这一步的结果会在特征提取中用到。

    void findStartEndAngle(){
        // start and end orientation of this cloud
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
            segMsg.endOrientation -= 2 * M_PI;
        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
    }

3. projectPointCloud

这一步把点云投影到RangeImage上,也就是说得到的原始点云是一个球形,我们把球形投影到一个圆柱体上然后把它展开,下图是一个简单的示例。

以16线激光为例,转换的range图,长为16,宽为旋转一周的采样次数,这里为1800,因此最后得到的是16*1800的图像。这个图像可以用数组表示,其中的坐标为x,y,而z会转换为深度信息,最后转换好的rangeImage效果如下图,类似一个深度图。

了解了原理之后我们下面开始分析代码

void projectPointCloud(){
        // range image projection
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();
        // 1. 遍历点云
        for (size_t i = 0; i < cloudSize; ++i){
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            // 2.1 如果有ring index则直接采用
            if (useCloudRing == true){
                rowIdn = laserCloudInRing->points[i].ring;
            }
            else{
                // 2.2 如果没有则通过计算角度得到index
                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
            }
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            // 3. 计算横坐标
            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;
            // 4. 计算距离,如果小于1M则过滤掉,通常用来过滤自身形成的点云
            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < sensorMinimumRange)
                continue;
            // 5. 保存距离到矩阵rangeMat
            rangeMat.at<float>(rowIdn, columnIdn) = range;
            // 6. 这里的强度实际上为了保持纵坐标和横坐标
            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
            // 7. 把点云保存到数组,fullCloud的强度为横纵坐标,fullInfoCloud中的为距离
            index = columnIdn  + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;
            fullInfoCloud->points[index] = thisPoint;
            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
        }
    }

4. groundRemoval

接下来的处理都是基于rangeImage,去除地面的过程如下,对rangeImage中行Ringindex小于7的点进行地面检测,如果是32线雷达,则是20以下的,所以这里是利用了Lidar的先验知识。

检测地面是判断rangeImage中的点和它相邻纵向的点的角度是否小于10°,如果小于10°则表示为地面。

void groundRemoval(){
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;
        // groundMat
        // -1, no valid info to check if ground of not
        //  0, initial value, after validation, means not ground
        //  1, ground
        // 1. 遍历小于7的点
        for (size_t j = 0; j < Horizon_SCAN; ++j){
            for (size_t i = 0; i < groundScanInd; ++i){

                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    // no info to check, invalid points
                    groundMat.at<int8_t>(i,j) = -1;
                    continue;
                }
                    
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
                // 2. 计算垂直的2个点的夹角
                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
                // 3. 如果小于10°则表示为地面
                if (abs(angle - sensorMountAngle) <= 10){
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }
            }
        }
        // extract ground cloud (groundMat == 1)
        // mark entry that doesn't need to label (ground and invalid point) for segmentation
        // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
        // 4. 如果为地面或者rangeMat为空,则标记为-1,后面去除这些点
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
                    labelMat.at<int>(i,j) = -1;
                }
            }
        }
        // 5. 这一步主要是为了发布地面点云
        if (pubGroundCloud.getNumSubscribers() != 0){
            for (size_t i = 0; i <= groundScanInd; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    if (groundMat.at<int8_t>(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                }
            }
        }
    }

5. cloudSegmentation

在去除地面之后,对接下来的点进行分割。这里递归进行分割,从[0,0]点开始,遍历它前后左右的4个点,分别进行对比,如果相对角度大于60°,则标记为有用的分割。最后分割的点的数量大于30个则有效(实际上大于5个可能也行)。

这里对cloudSegmentation拆分成2部分进行介绍,先进行标记labelComponents,也就是递归找到特征比较明显的点。

    void cloudSegmentation(){
        // segmentation process
        for (size_t i = 0; i < N_SCAN; ++i)
            for (size_t j = 0; j < Horizon_SCAN; ++j)
                // 1. 如果没有标记过,则遍历该节点(递归调用)
                if (labelMat.at<int>(i,j) == 0)
                    labelComponents(i, j);

下面来我们先分析labelComponents,然后再继续介绍cloudSegmentation。

作者说用vector和deque会变慢速度,不知道是不是数组的动态扩展导致的(当预先分配的vector大小不够时候,会重新malloc2倍的大小,然后把原先的数组拷贝过去,系统自动完成),如果设置固定大小,我认为不会导致明显的性能下降。这里作者用4个数组来代替了queue的操作,看起来有点别扭。

void labelComponents(int row, int col){
        // use std::queue std::vector std::deque will slow the program down greatly
        float d1, d2, alpha, angle;
        int fromIndX, fromIndY, thisIndX, thisIndY; 
        bool lineCountFlag[N_SCAN] = {false};

        queueIndX[0] = row;
        queueIndY[0] = col;
        int queueSize = 1;
        int queueStartInd = 0;
        int queueEndInd = 1;

        allPushedIndX[0] = row;
        allPushedIndY[0] = col;
        int allPushedIndSize = 1;
        // 1. 初始值为[row, col],长度为1
        while(queueSize > 0){
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // Mark popped point
            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
            // Loop through all the neighboring grids of popped grid
            // 2.遍历前后左右4个点,计算角度差
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
                // new index
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;
                // index should be within the boundary
                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;
                // at range image margin (left or right side)
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                    continue;

                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));
                // 2.1 纵向角度和横向角度
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
                // 2.2 如果角度大于60度 
                if (angle > segmentTheta){

                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    ++queueSize;
                    ++queueEndInd;

                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;

                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }
        // check if this segment is valid
        bool feasibleSegment = false;
        // 3. 如果点大于30个,认为成功
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        // 4. 如果点大于5个,并且横跨3个纵坐标,认为成功
        else if (allPushedIndSize >= segmentValidPointNum){
            int lineCount = 0;
            for (size_t i = 0; i < N_SCAN; ++i)
                if (lineCountFlag[i] == true)
                    ++lineCount;
            if (lineCount >= segmentValidLineNum)
                feasibleSegment = true;            
        }
        // segment is valid, mark these points
        // 5. 如果为真,则labelCount加1,labelCount代表分割为了多少个簇
        if (feasibleSegment == true){
            ++labelCount;
        }else{ // segment is invalid, mark these points
            // 6. 如果为假,那么标记为999999
            for (size_t i = 0; i < allPushedIndSize; ++i){
                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

接下来继续分析cloudSegmentation,主要是根据上述标记好的点云,生成分割好的点云。

        int sizeOfSegCloud = 0;
        // extract segmented cloud for lidar odometry
        for (size_t i = 0; i < N_SCAN; ++i) {

            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

            for (size_t j = 0; j < Horizon_SCAN; ++j) {
                if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
                    // outliers that will not be used for optimization (always continue)
                    // 1. 如果label为999999则跳过
                    if (labelMat.at<int>(i,j) == 999999){
                        if (i > groundScanInd && j % 5 == 0){
                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                            continue;
                        }else{
                            continue;
                        }
                    }
                    // majority of ground points are skipped
                    // 2. 如果为地,跳过index不是5的倍数的点
                    if (groundMat.at<int8_t>(i,j) == 1){
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    // 3. 这里有可能是地面,也有可能是分割之后的点
                    // mark ground points so they will not be considered as edge features later
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    // mark the points' column index for marking occlusion later
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    // save range info
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                    // save seg cloud
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of seg cloud
                    ++sizeOfSegCloud;
                }
            }

            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }
        
        // extract segmented cloud for visualization
        // 4. 分割后的点云,不包含地面
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
            for (size_t i = 0; i < N_SCAN; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
                    }
                }
            }
        }
    }

6. publishCloud

接下来就是发布分割好的点云,这个函数非常简单,主要是要搞清楚各个点云的作用,代码如下

void publishCloud(){
        // 1. Publish Seg Cloud Info
        // 1. 发布点云,包括采样后的地面和分割后的点云
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);
        // 2. Publish clouds
        // 2. 发布离群后的点云
        sensor_msgs::PointCloud2 laserCloudTemp;

        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubOutlierCloud.publish(laserCloudTemp);
        // segmented cloud with ground
        // 3. 同1
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);
        // projected full cloud
        // 4. rangeimage投影后的点云
        if (pubFullCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullCloud.publish(laserCloudTemp);
        }
        // original dense ground cloud
        //  5. 地面点云
        if (pubGroundCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*groundCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubGroundCloud.publish(laserCloudTemp);
        }
        // segmented cloud without ground
        // 6. 分割后的点云,不包含地面
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
            pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubSegmentedCloudPure.publish(laserCloudTemp);
        }
        // projected full cloud info
        // 7. rangeimage投影后的点云
        if (pubFullInfoCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullInfoCloud.publish(laserCloudTemp);
        }
    }

7. resetParameters

最后一步是清空临时变量,没有太多需要说明的。

    void resetParameters(){
        laserCloudIn->clear();
        groundCloud->clear();
        segmentedCloud->clear();
        segmentedCloudPure->clear();
        outlierCloud->clear();

        rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
        groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
        labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
        labelCount = 1;

        std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
        std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
    }

总结

至此整个LeGO-LOAM点云分割的部分就介绍完毕了,如果需要自己适配,需要关注下"LeGO-LOAM/include/utility.h"中不同型号Lidar的参数,比如夹角和地面查找的线束。

下一章,我们会接着介绍特征提取。

参考

https://github/RobustFieldAutonomyLab/LeGO-LOAM

本文仅做学术分享,如有侵权,请联系删文。

下载1

在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

重磅!3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近2000星球成员为创造更好的AI世界共同进步,知识星球入口:

学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用,麻烦给个赞和在看~  

本文标签: 一文详解LOAMLego