您的位置:首页 > 编程语言 > C语言/C++

LeGo-LOAM源码阅读笔记(二)---imageProjecion.cpp

2020-08-16 16:30 4647 查看

文章目录

1. imageProjecion.cpp概述

imageProjecion.cpp进行的数据处理是图像映射,将得到的激光数据分割,并在得到的激光数据上进行坐标变换。下列是在投影过程中主要用到的点云:

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;

2.构造函数

imageProjecion()构造函数的内容如下:

  1. 订阅话题:订阅来自velodyne雷达驱动的topic ,订阅的subscriber是

    subLaserCloud

    “/velodyne_points”
    (
    sensor_msgs::PointCloud2
    )
  • 发布话题,这些topic有:

      "/full_cloud_projected
      (
      sensor_msgs::PointCloud2
      )
    1. “/full_cloud_info”
      (
      sensor_msgs::PointCloud2
      )
    2. “/ground_cloud”
      (
      sensor_msgs::PointCloud2
      )
    3. “/segmented_cloud”
      (
      sensor_msgs::PointCloud2
      )
    4. “/segmented_cloud_pure”(sensor_msgs::PointCloud2
      )
    5. “/segmented_cloud_info”
      (
      cloud_msgs::cloud_info
      )
    6. “/outlier_cloud”
      (
      sensor_msgs::PointCloud2
      )

    然后分配内存(对智能指针初始化),初始化各类参数。

    上述的

    cloud_msgs::cloud_info
    是自定义的消息类型,其具体定义如下:

    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

    关于上面的自定义消息,另外还需要说明的是,

    segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5
    ;或者
    segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5
    ;表示的是将第0线和第16线点云横着排列后。每一线点云有一个
    startRingIndex
    endRingIndex
    ,表示这一线点云中的一部分,如下图绿色部分。
    黑色部分是整体这一线点云中筛选出来满足条件的。

    3.cloudHandler

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    是这个文件中最主要的一个函数。由它调用其他的函数:

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    copyPointCloud(laserCloudMsg);
    findStartEndAngle();
    projectPointCloud();
    groundRemoval();
    cloudSegmentation();
    publishCloud();
    resetParameters();
    }

    整体过程如下:
    1.复制点云数据
           点云数据函数copyPointCloud(laserCloudMsg) 是将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。
    2.找到开始和结束的角度
    3.点云投影
    4.地面检测,移除地面点
    5.点云分割
    6.发布处理后的点云数据
    7.重置参数,清空此次的点云变量

    4.velodyne 雷达数据

    从VLP给的雷达数据手册上(63-9243 Rev B User Manual and Programming Guide,VLP-16.pdf)查找一下它的坐标系定义:

    velodyne雷达在上面的坐标系下输出

    "/velodyne_points"
    (
    sensor_msgs::PointCloud2
    ) 的点云数据,其数据格式可以理解为x,y,z,intensity 这4个量。

    5.findStartEndAngle

    void findStartEndAngle()是将起始点与最末点进行角度的转换,进行关于

    segMsg
    (
    cloud_msgs::cloud_info segMsg
    )的三个内容的计算:
    1)计算开始角度(
    segMsg.startOrientation
    );
    2)计算结束角度(
    segMsg.endOrientation
    );
    3)计算雷达转过的角度(开始和结束的角度差,
    segMsg.orientationDiff
    )。

    关于具体计算,需要清楚整个雷达的坐标系定义,参考上面雷达坐标系的那张图。

    另外在计算

    segMsg.startOrientation
    segMsg.endOrientation
    时,atan2()函数取了负数的原因是:雷达旋转方向和坐标系定义下的右手定则正方向不一致。参考下图:


    代码如下:

    //将起始点与最末点进行角度的转换
    void findStartEndAngle()
    {
    //计算角度时以x轴负轴为基准
    segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
    //因此最末角度为2π减去计算值
    segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 2].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;
    }

    6.projectPointCloud

    void projectPointCloud()
    是将点云逐一计算深度,将具有深度的点云保存至fullInfoCloud中。

    void projectPointCloud()
    将激光雷达得到的数据看成一个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

      这里对数据的处理比较巧妙,处理的目的是让数据更不容易失真。
      计算
      columnIdn
      主要是下面这三个语句:
    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;

    先把

    columnIdn
    horizonAngle:(-PI,PI]
    转换到
    columnIdn:[H/4,5H/4]
    ,然后判断
    columnIdn
    大小,再将它的范围转换到了
    [0,H] (H:Horizon_SCAN)

    这样就把扫描开始的地方角度为0与角度为360的连在了一起,非常巧妙。
    5. 接着在
    thisPoint.intensity
    中保存一个点的位置信息
    rowIdn+columnIdn / 10000.0
    fullInfoCloud
    的点保存点的距离信息;

    具体的转换过程可以看下面这个两张图:

    代码如下:

    //逐一计算点云深度,并具有深度的点云保存至fullInfoCloud中
    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矩阵中保存该点的深度,保存单通道像素值
    rangeMat.at<float>(rowIdn, columnIdn) = range;
    
    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"
    }
    }

    7.各种标记的含义

    • 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。

    8.groundRemoval

    void groundRemoval()
    是利用不同的扫描圈来表示地面,进而检测地面是否水平。例如在源码中的七个扫描圈,每两个圈之间进行一次比较,角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云。

    void groundRemoval()
    由三个部分的程序组成。

    1. 由上下两线之间点的XYZ位置得到两线之间的俯仰角,如果俯仰角在10度以内,则判定(i,j)和(i,j+1)为地面点,
      groundMat[i][j]=1
      ,否则,则不是地面点,进行后续操作;
    2. 找到所有点中的地面点,并将他们的
      labelMat
      标记为-1,
      rangeMat[i][j]==FLT_MAX
      判定为无效的另一个条件。
    3. 如果有节点订阅
      groundCloud
      ,那么就需要把地面点发布出来。具体实现过程:把点放到
      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]);
    }
    }
    }
    }

    9. cloudSegmentation

    void cloudSegmentation()
    作为本程序的关键部分,首先调用了labelComponents函数,该函数对特征的检测进行了详细的描述,并且是针对于某一特定的点与其邻点的计算过程。

    void cloudSegmentation()
    进行的是点云分割的操作,将不同类型的点云放到不同的点云块中去,例如
    outlierCloud
    segmentedCloudPure
    等。具体步骤:

    1. 首先判断点云标签,这个点云没有进行过分类(在原先的处理中没有被分到地面点中去或没有分到平面中),则通过
      labelComponents(i, j)
      ;对点云进行分类。进行分类的过程在
      labelComponents
      函数中进行介绍。
    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;
    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;
    }
    }
    if (groundMat.at<int8_t>(i,j) == 1)
    {
    //地面点云每隔5个点纳入被分割点云
    if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
    continue;
    }
    //segMsg是自定义rosmsg
    //是否是地面点
    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
    //当前水平方向上的行数
    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
    //深度
    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
    //把当前点纳入分割点云中
    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
    ++sizeOfSegCloud;
    }
    }
    
    segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
    }
    //如果在当前有节点订阅便将分割点云的几何信息也发布出去
    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);
    }
    }
    }
    }
    }

    10.labelComponents

    void labelComponents(int row, int col)
    对点云进行标记。通过标准的BFS算法对点进行标记:以(row,col)为中心向外面扩散,判断(row,col)是否属于平面中一点。

    • BFS过程:
    1. queueIndX
      queueIndY
      保存进行分割的点云行列值,用
      queueStartInd
      作为索引。
    2. 求这个点的4个邻接点,求其中离原点距离的最大值
      d1
      最小值
      d2
      。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样,所以
      alpha
      是用来选择分辨率的。
    // alpha代表角度分辨率,
    // Y方向上角度分辨率是segmentAlphaY(rad)
    if ((*iter).first == 0)
    alpha = segmentAlphaX;
    else
    alpha = segmentAlphaY;
    
    // 通过下面的公式计算这两点之间是否有平面特征
    // atan2(y,x)的值越大,d1,d2之间的差距越小,越平坦
    angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
    • 在这之后通过判断角度是否大于60度来决定是否要将这个点加入保存的队列。加入的话则假设这个点是个平面点。
      -然后进行聚类,聚类的规则是:
    1. 如果聚类超过30个点,直接标记为一个可用聚类,labelCount需要递增;
    2. 如果聚类点数小于30大于等于5,统计竖直方向上的聚类点数
    3. 竖直方向上超过3个也将它标记为有效聚类
    4. 标记为999999的是需要舍弃的聚类的点,因为他们的数量小于30个

    代码如下:

    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;
    // Loop through all the neighboring grids of popped grid
    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));
    
    //该迭代器的first是0则是水平方向上的邻点,否则是竖直方向上的
    // alpha代表角度分辨率,
    // Y方向上角度分辨率是segmentAlphaY(rad)
    if ((*iter).first == 0)
    alpha = segmentAlphaX;
    else
    alpha = segmentAlphaY;
    
    // 通过下面的公式计算这两点之间是否有平面特征
    // atan2(y,x)的值越大,d1,d2之间的差距越小,越平坦
    //这个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;
    
    allPushedIndX[allPushedIndSize] = thisIndX;
    allPushedIndY[allPushedIndSize] = thisIndY;
    ++allPushedIndSize;
    }
    }
    }
    
    // check if this segment is valid
    bool feasibleSegment = false;
    //当邻点数目达到30后,则该帧雷达点云的几何特征配置成功
    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<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
    }
    }
    }

    11.publishCloud

    void publishCloud()
    发布各类点云数据,在
    publishCloud
    函数中可以看到,计算的过程中参考系均为机器人自身参考系,
    frame_id
    自然
    base_link

    代码如下:

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

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

    13.主函数

    代码如下:

    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;
    }
  • 内容来自用户分享和网络整理,不保证内容的准确性,如有侵权内容,可联系管理员处理 点击这里给我发消息
    标签: