天天看點

LeGO-LOAM代碼和原了解析(1)-Image Projection前言一、ImageProjection成員變量二、構造函數ImageProjection()三、回調函數cloudHandler總結

文章目錄

  • 前言
  • 一、ImageProjection成員變量
  • 二、構造函數ImageProjection()
    • 1.配置設定記憶體allocateMemory
  • 三、回調函數cloudHandler
    • 1.copyPointCloud
    • 2.findStartEndAngle
    • 3.projectPointCloud
    • 4.groundRemoval
    • 5.cloudSegmentation
    • 6.labelComponents
    • 6.resetParameters
  • 總結

前言

imageProjecion.cpp進行的資料處理是圖像映射,将得到的雷射資料分割,并在得到的雷射資料上進行坐标變換。

一、ImageProjection成員變量

pcl::PointCloud<PointType>::Ptr laserCloudIn;//接受到的來自雷射Msg的原始點雲資料  
pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;//用 laserCloudInRing 存儲含有具有通道R的原始點雲資料  
//深度圖點雲:以一維形式存儲與深度圖像素一一對應的點雲資料  
pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix  
//帶距離值的深度圖點雲:與深度圖點雲存儲一緻的資料,但是其屬性intensity記錄的是距離值  
pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range  
//注:所有點分為被分割點、未被分割點、地面點、無效點。  
pcl::PointCloud<PointType>::Ptr groundCloud;//地面點點雲  
pcl::PointCloud<PointType>::Ptr segmentedCloud;//segMsg 點雲資料:包含被分割點和經過降采樣的地面點  
pcl::PointCloud<PointType>::Ptr segmentedCloudPure;//存儲被分割點點雲,且每個點的i值為分割标志值  
pcl::PointCloud<PointType>::Ptr outlierCloud;//經過降采樣的未分割點  
           

另外使用自創的rosmsg來表示點雲資訊

// segMsg點雲資訊(存儲分割結果并用于發送)  
cloud_msgs::cloud_info segMsg; 
           

具體定義如下:

Header header  
int32[] startRingIndex  // 長度:N_SCAN  
int32[] endRingIndex    // 長度:N_SCAN  
float32 startOrientation  
float32 endOrientation  
float32 orientationDiff  
// 以下長度都是 N_SCAN*Horizon_SCAN  
bool[]    segmentedCloudGroundFlag # true - ground point, false - other points  
uint32[]  segmentedCloudColInd # point column index in range image  
float32[] segmentedCloudRange # point range  
           

二、構造函數ImageProjection()

main()函數通過構造函數訂閱和釋出消息。代碼如下:

subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>
        (pointCloudTopic, 1, &ImageProjection::cloudHandler, this);

    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);
    ...
    allocateMemory();//為指針配置設定記憶體,否則出現野指針錯誤,初始化一次
    resetParameters();//容器清空,參數初始化化,為下一次循環做準備,回調函數中循環
           

其中pointCloudTopic就是輸入點雲的topic,本代碼中為/os1_points 。

1.配置設定記憶體allocateMemory

初始化:為成員變量指針配置設定記憶體,否則出現野指針錯誤,在構造函數中初始化一次。

代碼如下:

void allocateMemory(){

    laserCloudIn.reset(new pcl::PointCloud<PointType>());
    laserCloudInRing.reset(new pcl::PointCloud<PointXYZIR>());

    fullCloud.reset(new pcl::PointCloud<PointType>());
    fullInfoCloud.reset(new pcl::PointCloud<PointType>());

    groundCloud.reset(new pcl::PointCloud<PointType>());
    segmentedCloud.reset(new pcl::PointCloud<PointType>());
    segmentedCloudPure.reset(new pcl::PointCloud<PointType>());
    outlierCloud.reset(new pcl::PointCloud<PointType>());

    fullCloud->points.resize(N_SCAN*Horizon_SCAN);
    fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);

    segMsg.startRingIndex.assign(N_SCAN, 0);
    segMsg.endRingIndex.assign(N_SCAN, 0);

    segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);
    segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);
    segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);

    std::pair<int8_t, int8_t> neighbor;//上下左右偏移
    neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
    neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
    neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
    neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);

    allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
    allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];

    queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
    queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
  }
           

三、回調函數cloudHandler

代碼會在回調函數内進行循環,需注意成員變量容器的清空,否則會記憶體洩露,本代碼由構造函數中resetParameters()函數進行clear

由它調用其他的函數。代碼如下:

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();//容器清空,參數初始化化,為下一次循環做準備
   }
           

1.copyPointCloud

本函數将ROS中的sensor_msgs::PointCloud2ConstPtr類型轉換到pcl點雲庫指針。代碼如下:

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    cloudHeader = laserCloudMsg->header;
    cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
    // Remove Nan points
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
    // have "ring" channel in the cloud
    if (useCloudRing == true){
      pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
      if (laserCloudInRing->is_dense == false)// 指定points中的資料是否全部是有限的,此時為true;當資料集中包含有Inf/NaN等值時,此時為false。
      {
        ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
        ros::shutdown();
      }
    }
  }
           

2.findStartEndAngle

本函數将起始點與最末點進行角度的轉換

并儲存到segMsg:

1)計算開始角度(segMsg.startOrientation);

2)計算結束角度(segMsg.endOrientation);

3)計算雷達轉過的角度(開始和結束的角度差,segMsg.orientationDiff)。

代碼如下(示例):

//将起始點與最末點進行角度的轉換
  void findStartEndAngle(){
    //計算角度時以x軸負軸為基準(-pi~-0~0~pi)
    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;
  }
           

下圖依次是atan2和-atan的取值範圍,以及正常和特殊情況下開始角度和結束角度

LeGO-LOAM代碼和原了解析(1)-Image Projection前言一、ImageProjection成員變量二、構造函數ImageProjection()三、回調函數cloudHandler總結

3.projectPointCloud

本函數逐一計算點的深度,将具有深度的點雲儲存至fullInfoCloud中。

以16為例,将雷射雷達得到的資料看成一個16x1800的點雲陣列。然後根據每個點雲傳回的XYZ資料将他們對應到這個陣列裡去。

1)計算豎直角度,用atan2函數進行計算。

2)通過計算的豎直角度得到對應的行的序号rowIdn,rowIdn計算出該點雷射雷達是水準方向上第幾線的。從下往上計數,-15度記為初始線,第0線,一共16線(N_SCAN=16)。

3)求水準方向上的角度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI。

4)根據水準方向上的角度計算列向columnIdn。

5. 接着在thisPoint.intensity中儲存一個點的位置資訊rowIdn+columnIdn / 10000.0,存入fullInfoCloud點雲。

代碼如下:

//逐一計算點雲深度,并具有深度的點雲儲存至fullInfoCloud中;本函數将整個點雲資料投影成一個16*1800的二維平面圖
  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 = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
      //round是四舍五入取偶
      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.at<float>(rowIdn, columnIdn) = range;
      // 每個點的強度值,整數部分代表行号,小數部分代表列号。後面會通過intensity來恢複這個點
      thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

      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

本函數利用不同的掃描圈來表示地面,進而檢測地面是否水準。例如在源碼中的七個掃描圈,每兩個圈之間進行一次比較,角度相差10°以内的我們可以看做是平地。并且将掃描圈中的點加入到groundCloud點雲。

代碼如下:

//利用不同的掃描圈來表示地面,進而檢測地面是否水準。例如在源碼中的七個掃描圈,每兩個圈之間
  //進行一次比較,角度相差10°以内的我們可以看做是平地。并且将掃描圈中的點加入到groundCloud點雲
  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<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;

        angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

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

各種标記的含義

groundMat:
1) groundMat.at<int8_t>(i,j) = 0,初始值;
2) groundMat.at<int8_t>(i,j) = 1,有效的地面點;
3) groundMat.at<int8_t>(i,j) = -1,無效地面點;
rangeMat
1) rangeMat.at(i,j) = FLT_MAX,浮點數的最大值,初始化資訊;
2) rangeMat.at(rowIdn, columnIdn) = range,儲存圖像深度;
labelMat
1) labelMat.at(i,j) = 0,初始值;
2) labelMat.at(i,j) = -1,無效點;
3)labelMat.at(thisIndX, thisIndY) = labelCount,平面點;
4)labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999,需要舍棄的點,數量不到30。
           

5.cloudSegmentation

本函數作為本程式的關鍵部分,首先調用了labelComponents函數,該函數對特征的檢測進行了詳細的描述,并且是針對于某一特定的點與其鄰點的計算過程。

具體步驟:

1)首先判斷點雲标簽,這個點雲沒有進行過分類(在原先的進行中沒有被分到地面點中去或沒有分到平面中),則通過labelComponents(i, j);對點雲進行分類。

2)分類完成後,找到可用的特征點或者地面點(不選擇labelMat[i][j]=0的點),按照它的标簽值進行判斷,将部分界外點放進outlierCloud中。continue繼續處理下一個點。

3)然後将大部分地面點去掉,剩下的那些點進行資訊的拷貝與儲存操作。

4)最後如果有節點訂閱SegmentedCloudPure,那麼把點雲資料儲存到segmentedCloudPure中去。

代碼如下:

void cloudSegmentation(){
    //這是在排除地面點與異常點之後,逐一檢測鄰點特征并生成局部特征
    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)
          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<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
          //離群點或異常點的處理
          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
          if (groundMat.at<int8_t>(i,j) == 1){
            //地面點雲每隔5個點納入被分割點雲
            if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
              continue;
          }
          // 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
    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.labelComponents

本函數對點雲進行标記。通過标準的BFS算法對點進行标記:以(row,col)為中心向外面擴散,判斷(row,col)是否屬于平面中一點。

具體過程:

1)用queueIndX,queueIndY儲存進行分割的點雲行列值,用queueStartInd作為索引。

2)求這個點的4個鄰接點,求其中離原點距離的最大值d1最小值d2。根據下面這部分代碼來評價這兩點之間是否具有平面特征。注意因為兩個點上下或者水準對應的分辨率不一樣,是以alpha是用來選擇分辨率的。

3)通過判斷角度是否大于60度來決定是否要将這個點加入儲存的隊列,然後生長聚類。

4)如果聚類超過30個點,直接标記為一個可用聚類;如果聚類點數小于30大于等于5,統計豎直方向上的聚類點數,豎直方向上超過3個也将它标記為有效聚類。

代碼如下:

//首先調用了labelComponents函數,
  //該函數對特征的檢測進行了詳細的描述,
  //并且是針對于某一特定的點與其鄰點的計算過程。
  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;
    //queueSize指的是在特征處理時還未處理好的點的數量,
    //是以該while循環是在嘗試檢測該特定點的周圍的點的幾何特征
    while(queueSize > 0){
      // Pop point
      fromIndX = queueIndX[queueStartInd];
      fromIndY = queueIndY[queueStartInd];
      --queueSize;
      ++queueStartInd;
      // Mark popped point
      labelMat.at<int>(fromIndX, fromIndY) = labelCount;
      //檢查上下左右四個鄰點
      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與d2分别是該特定點與某鄰點的深度
        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));
        //該疊代器的first是0則是水準方向上的鄰點,否則是豎直方向上的,設定相應的角分辨率。
        if ((*iter).first == 0)
          alpha = segmentAlphaX;
        else
          alpha = segmentAlphaY;
        //這個angle其實是該特定點與某鄰點的連線與XOZ平面的夾角,
        //這個夾角代表了局部特征的敏感性
        angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
        //如果夾角大于60°,則将這個鄰點納入到局部特征中,該鄰點可以用來配準使用
        if (angle > segmentTheta){

          queueIndX[queueEndInd] = thisIndX;
          queueIndY[queueEndInd] = thisIndY;
          ++queueSize;
          ++queueEndInd;
          
          labelMat.at<int>(thisIndX, thisIndY) = labelCount;
          lineCountFlag[thisIndX] = true; // 聚類中包括thisIndX環

          allPushedIndX[allPushedIndSize] = thisIndX;
          allPushedIndY[allPushedIndSize] = thisIndY;
          ++allPushedIndSize;
        }
      }
    }
    // check if this segment is valid
    bool feasibleSegment = false;
    // 聚類超過30個點,直接标記為一個可用聚類
    if (allPushedIndSize >= 30)
      feasibleSegment = true;
    //如果聚類點數小于30大于等于5,統計豎直方向上的聚類點數
    else if (allPushedIndSize >= segmentValidPointNum){
      int lineCount = 0;
      for (size_t i = 0; i < N_SCAN; ++i)
        if (lineCountFlag[i] == true)
          ++lineCount;
      //豎直方向上超過3個也将它标記為有效聚類
      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<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
      }
    }
  }
           

其中d1、d2、alpha和angle的關系如下圖所示:

LeGO-LOAM代碼和原了解析(1)-Image Projection前言一、ImageProjection成員變量二、構造函數ImageProjection()三、回調函數cloudHandler總結

6.resetParameters

因為循環體在類的成員函數裡面,是以需要對容器類成員變量進行clear操作,以及在每次循環後對一些成員變量重新初始化。

代碼如下:

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

該處使用的url網絡請求的資料。

總結

本節點對每一幀的雷射資料的預處理。包括了地面的提取(沒有統一平面作為地面的假設),點雲的實時分割。

繼續閱讀