LeGo-Loam代码解析ImageProjection节点（一）

LeGo-Loam代码解析ImageProjection节点（一） 1.  定义ImageProjection 对象IP

subLaserCloud = nh.subscribe(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
pubFullCloud = nh.advertise ("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise ("/full_cloud_info", 1);
pubGroundCloud = nh.advertise ("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise ("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise ("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise ("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise ("/outlier_cloud", 1);
nanPoint.x = std::numeric_limits::quiet_NaN();
nanPoint.y = std::numeric_limits::quiet_NaN();
nanPoint.z = std::numeric_limits::quiet_NaN();
nanPoint.intensity = -1;
//分配内存
allocateMemory();
//初始化参数
resetParameters();

2. 订阅点云，进入回调函数cloudHandler()

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

    // 1. Convert ros message to pcl point cloud
    copyPointCloud(laserCloudMsg);
    // 2. Start and end angle of a scan
    findStartEndAngle();
    // 3. Range image projection
    projectPointCloud();
    // 4. Mark ground points
    groundRemoval();
    // 5. Point cloud segmentation
    cloudSegmentation();
    // 6. Publish all clouds
    publishCloud();
    // 7. Reset parameters for next iteration
    resetParameters();
}

3. copyPointCloud函数，将点云转换为pcl类型（百度网盘中的驱动转换为pcl时带有ring和tm），

    此时laserCloudIn为去除无效点之后的点云。

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

        cloudHeader = laserCloudMsg->header;
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
        // Remove Nan points
        std::vector indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
        // have "ring" channel in the cloud
        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();
            }  
        }
    }

4. findStartEndAngle函数，计算一帧点云的起始角和终止角。

        4.1 velodyne激光器的坐标系，以及线ID的排序

 

 

 

     

        4.2. 代码中使用atan2(y,x),求的是扫描点相对于原点的角度（即扫描的起始角度，以x轴为起始轴，按顺时针进行扫描，而实际坐标系为右手定则，所以需要加上负号），所求解的startOrientation，endOrientation，orientationDiff并未使用

 

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;
    }

5. projectPointCloud函数，将点云投影成图片的方式，线id作为该像素点的行，通过扫描的角分辨率去确定该像素点的列，

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

        cloudSize = laserCloudIn->points.size();

        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;
            // find the row and column index in the iamge for this point
            if (useCloudRing == true){
                rowIdn = laserCloudInRing->points[i].ring;
            }
            else{
                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的角度范围为[-180, 180]
            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            // horizonAngle-90 将角度范围变为[-270, 90], round之后将范围变为[3/4H, -1/4H]
            // 再加上1/2H, 使columnIdn的变为[5/4H， 1/4H]
            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;

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < sensorMinimumRange)
                continue;
            //将点云的距离存放在rangeMat中
            rangeMat.at(rowIdn, columnIdn) = range;
            //强度的整数部分存在线id，小数部分存放像素点的列
            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
            //像素点的索引号
            index = columnIdn  + rowIdn * Horizon_SCAN;
            //fullCloud和fullInfoCloud两者存放点的区别在于强度的存储值，后者存放的是距离
            fullCloud->points[index] = thisPoint;
            fullInfoCloud->points[index] = thisPoint;
            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
        }
    }

6. groundRemoval 地面点的检测, 这个地方可以对比hdl_graph_slam和mulls中地面点清除

    6.1.   lego-loam地面点检测：计算上下两根线对应点的俯仰角atan2(z, sqrt(x^2+y^2)),如果角度               在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
      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(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;
          
          angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
          
          if (abs(angle - sensorMountAngle) <= 10){
            groundMat.at(i,j) = 1;
            groundMat.at(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
      for (size_t i = 0; i < N_SCAN; ++i){
        for (size_t j = 0; j < Horizon_SCAN; ++j){
          if (groundMat.at(i,j) == 1 || rangeMat.at(i,j) == FLT_MAX){
            labelMat.at(i,j) = -1;
          }
        }
      }
      if (pubGroundCloud.getNumSubscribers() != 0){
        for (size_t i = 0; i <= groundScanInd; ++i){
          for (size_t j = 0; j < Horizon_SCAN; ++j){
            if (groundMat.at(i,j) == 1)
              groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
          }
        }
      }
      if(scan_num == 10)
      {
        for (size_t i = 0; i <= groundScanInd; ++i){
          for (size_t j = 0; j < Horizon_SCAN; ++j){
            if (groundMat.at(i,j) == 1)
              outfile1 << fullCloud->points[j + i*Horizon_SCAN].x << " " << fullCloud->points[j + i*Horizon_SCAN].y << " " << fullCloud->points[j + i*Horizon_SCAN].z << endl;
          }
        }
      }
    }
    

    地面点提取的效果图:

       

 

    6.2.    hdl_graph_slam地面点检测：根据高度参数将地面分割出来，然后使用法向量过滤，最                  后通过采用Ransac平面约束来筛选

   void groundRemoval_hdl()
    {
      Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
      tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, 
      Eigen::Vector3f::UnitY()).toRotationMatrix();

      // filtering before RANSAC (height and normal filtering)
      if(scan_num == 10)
      {
        pcl::io::savePLYFileASCIi(testDirectory+"before.ply", *fullCloud);
      }

      pcl::PointCloud::Ptr filtered(new pcl::PointCloud());
      pcl::transformPointCloud(*fullCloud, *filtered, tilt_matrix);
      //false get the upward
      filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
      //true get the down
      filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);



      if(scan_num == 10)
      {
        std::cout << "num:" << filtered->points.size() << endl;
        pcl::io::savePLYFileASCIi(testDirectory+"after.ply", *filtered);
      }
      if(use_normal_filtering) {
            filtered = normal_filtering(filtered);
          }
      if(scan_num == 10)
      {
        pcl::io::savePLYFileASCIi(testDirectory+"normal_filter.ply", *filtered);
      }

      pcl::SampleConsensusModelPlane::Ptr model_p(new 
      pcl::SampleConsensusModelPlane(filtered));
      pcl::RandomSampleConsensus ransac(model_p);
      ransac.setDistanceThreshold(0.1);
      ransac.computeModel();

      pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
      ransac.getInliers(inliers->indices);

      // too few inliers
      if(inliers->indices.size() < floor_pts_thresh) {
        return ;
      }

      // verticality check of the detected floor's normal
      Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();

      Eigen::VectorXf coeffs;
      ransac.getModelCoefficients(coeffs);

      double dot = coeffs.head<3>().dot(reference.head<3>());
      if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
        // the normal is not vertical
        return ;
      }

      // make the normal upward
      if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
        coeffs *= -1.0f;
      }

      if(scan_num == 10) {
        pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud);
        pcl::ExtractIndices extract;
        extract.setInputCloud(filtered);
        extract.setIndices(inliers);
        extract.filter(*inlier_cloud);

        if(scan_num == 10)
        {
          pcl::io::savePLYFileASCIi(testDirectory+"ransac_filter.ply", *inlier_cloud);
        }

      }


    }

    地面点的提取效果为：

   

 

    6.3     MULLS地面点检测：通过设置两个阈值来进行筛选

         地面点的提取效果为(MULLS 里面有很多参数都可以调节)：

        对比三种方法hdl和MULLS的方法要远比lego-loam的好，都采用了RANSAC进行约束，通过高度过滤进行地面提取。

7. cloudSegmentation点云分割，首先对每个点执行labelComponents函数，使用BFS来对点云进行分类，

        BFS的讲解可以查看搜索思想——DFS & BFS（基础基础篇） - 知乎 (zhihu.com)，

        该部分的论文为：Fast Range Image-based Segmentation of Sparse 3D Laser Scans for online Operation。

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;
        
        while(queueSize > 0){
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // Mark popped point
            labelMat.at(fromIndX, fromIndY) = labelCount;
            //遍历该点周围的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;
				//labelMat 不等于零的点为已经分类之后的点
                if (labelMat.at(thisIndX, thisIndY) != 0)
                    continue;
				//β=arctan(d2*sinα/(d1 - d2cosα)) 其中α为两个点之间的夹角，对应垂直和水平方向的分辨率
                d1 = std::max(rangeMat.at(fromIndX, fromIndY), 
                              rangeMat.at(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at(fromIndX, fromIndY), 
                              rangeMat.at(thisIndX, thisIndY));

                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
				//若计算的角度大于segmentTheta，则认为该点和待分配点是同一类点，并把该点的像素坐标保存，便于下次将该点作为基准点
				//将该点的label值记为和待分配点一致
                if (angle > segmentTheta){

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

                    labelMat.at(thisIndX, thisIndY) = labelCount;
					//将该点所在线序记为true
                    lineCountFlag[thisIndX] = true;
					//将该点的像素值保存
                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }

        // check if this segment is valid
		// 同类的点数大于30或者同类点小于30大于5，并且这些点在至少3根线，则认为这些点为同类点
		//将feasibleSegment置为true，否则为false 并将label置为999999
        bool feasibleSegment = false;
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        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
        if (feasibleSegment == true){
            ++labelCount;
        }else{ // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i){
                labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

   cloudSegmentation，对分类后的点云进行处理，其中外点主要包括label值为999999并且该点的像素值u大于地面的线序值，自定义类型segMsg中，segmentedCloudGroundFlag为true则代表其为地面点 false为非地面点，segmentedCloudColInd为该像素点的v，segmentedCloudRange为该点的距离，segmentedCloud存放的是分类后的点云并舍弃了每条线前后的五个点，分类后的点存放在segmentedCloudPure中（不包含地面点）

void cloudSegmentation(){
        // segmentation process
        for (size_t i = 0; i < N_SCAN; ++i)
            for (size_t j = 0; j < Horizon_SCAN; ++j)
                if (labelMat.at(i,j) == 0)
                    labelComponents(i, j);

        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(i,j) > 0 || groundMat.at(i,j) == 1){
                    // outliers that will not be used for optimization (always continue)
					//999999代表的是无效点，outlierCloud不包含线id小于地面线的点，降采样，隔5个点采样一次
                    if (labelMat.at(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
                    if (groundMat.at(i,j) == 1){
                        if (j%5!=0 && j>5 && j(i,j) == 1);
                    // mark the points' column index for marking occlusion later
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    // save range info
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at(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
		//如果需要显示分割之后的点，则将其强度值信息赋值为类型号
        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(i,j) > 0 && labelMat.at(i,j) != 999999){
                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        segmentedCloudPure->points.back().intensity = labelMat.at(i,j);
                    }
                }
            }
        }
    }

8. 发布对应的点：

        pubFullCloud 为将点云投影为图像之后的所有点，其中强度信息保存的是uv值，u为整型部分，v为小数部分
        pubFullInfoCloud 和pubFullCloud类似，只是其强度信息中保存的是点的距离                pubGroundCloud 为地面点
        pubSegmentedCloud 分类后的点包括地面点
        pubSegmentedCloudPure 分类后的点云 不包括地面点;
        pubSegmentedCloudInfo  自定义类型的点云;
        pubOutlierCloud 外点;

void publishCloud(){
        // 1. Publish Seg Cloud Info
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);
        // 2. Publish clouds
        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
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);
        // projected full cloud
        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
        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
        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
        if (pubFullInfoCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullInfoCloud.publish(laserCloudTemp);
        }
    }