LeGo-LOAM源码阅读笔记(三)---featureAssociation.cpp

1.featureAssociation框架

1.1 节点代码主体

int main(int argc, char **argv) {
   
    ros::init(argc, argv, "lego_loam");

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

    FeatureAssociation FA;//调用构造函数进行初始化

    ros::Rate rate(200);
    while (ros::ok()) {
   
        ros::spinOnce();

        FA.runFeatureAssociation(); //200hz调用主体函数

        rate.sleep();
    }

    ros::spin();
    return 0;
}

1.2 FeatureAssociation构造函数

FeatureAssociation的构造函数初始化了一些ros话题的收发，接着就是一个死循环不断的调用runFeatureAssociation（以每秒200HZ的频率）。

作用:订阅分割点云和imu数据,存成类成员变量,为runFeatureAssociation处理新数据做准备。

//订阅话题:

    "/segmented_cloud"(sensor_msgs::PointCloud2)，数据处理函数laserCloudHandler
    "/segmented_cloud_info"(cloud_msgs::cloud_info)，数据处理函数laserCloudInfoHandler
    "/outlier_cloud"(sensor_msgs::PointCloud2)，数据处理函数outlierCloudHandler
    imuTopic = "/imu/data"(sensor_msgs::Imu)，数据处理函数imuHandler

//发布话题，这些topic有：

    "/laser_cloud_sharp"(sensor_msgs::PointCloud2)
    "/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)
    "/laser_cloud_flat"(sensor_msgs::PointCloud2)
    "/laser_cloud_less_flat"(sensor_msgs::PointCloud2)
    "/laser_cloud_corner_last"(sensor_msgs::PointCloud2)
    "/laser_cloud_surf_last"(cloud_msgs::cloud_info)
    "/outlier_cloud_last"(sensor_msgs::PointCloud2)
    "/laser_odom_to_init"(nav_msgs::Odometry)

然后初始化各类参数。

1.3 runFeatureAssociation()主体函数

void runFeatureAssociation()是特征提取和配准的主体。
算法步骤如下：

1. 如果有新数据进来则执行，否则不执行任何操作；
2. 将点云数据进行坐标变换，进行插补等工作；
3. 进行光滑性计算，并保存结果；
4. 标记阻塞点(阻塞点：点云中可能出现的互相遮挡的点)；
5. 特征抽取，然后分别保存cornerPointsSharp等队列中去；
6. 发布cornerPointsSharp等4种类型的点云数据；
7. 预测位姿；
8. 更新变换；
9. 积分总变换
10. 发布里程计信息及上一次点云信息；

  void runFeatureAssociation() {
   

        // 获得了三个点云主题的msg,且时间上一致,将标志位置0,继续接下来的处理
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {
   

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        } else {
   
            return;
        }
        /**
        	1. Feature Extraction
        */
        // 点云形状调整
        //主要进行的处理是将点云数据进行坐标变换，进行插补等工作
        adjustDistortion();

        // 计算曲率，并保存结果
        calculateSmoothness();

        // 标记瑕点,loam中:1.平行scan的点(可能看不见);2.会被遮挡的点
        markOccludedPoints();

        // 特征提取
        // 点云分类,然后分别保存到cornerPointsSharp等队列
        // 这一步中减少了点云数量，使计算量减少
        extractFeatures();

        // 发布cornerPointsSharp等4种类型的点云数据
        publishCloud(); // cloud for visualization

        /**
		2. Feature Association
        */
        //在配准之前，检验LM法是否初始化，接下来就是里程计部分
        if (!systemInitedLM) {
   
            checkSystemInitialization();
            return;
        }

        //提供粗配准的先验以供优化
        updateInitialGuess();

        //优化并发送里程计信息
        updateTransformation();

        integrateTransformation();

        publishOdometry();

        //发送点云以供建图使用
        publishCloudsLast(); // cloud to mapOptimization
    }
};

2. 回调函数

2.1 laserCloudHandler

订阅 "/segmented_cloud"的回调函数
作用: laserCloudHandler修改点云数据的时间戳，将点云数据从ROS定义的格式转化到pcl的格式。
pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);

/*
   主题"/segmented_cloud"的回调函数,接收到的是经过imageProjection.cpp处理后的点云:主要是被分割点和经过降采样的地面点,其 intensity 记录了该点在深度图上的索引位置
   由点云 segmentedCloud 接收
   同时
       cloudHeader 记录消息的header
       timeScanCur 记录header中的时间
       标志位 newSegmentedCloud 置为true
   */
    void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
   

        cloudHeader = laserCloudMsg->header;

        timeScanCur = cloudHeader.stamp.toSec();
        timeNewSegmentedCloud = timeScanCur;

        segmentedCloud->clear();
        pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);

        newSegmentedCloud = true;
    }

2.2 laserCloudInfoHandler

订阅 "/segmented_cloud_info"的触发回调函数laserCloudInfoHandler
newSegmentedCloudInfo = true;
作用:为runFeatureAssociation()做准备

 /*
       主题"/segmented_cloud_info"的回调函数
       timeNewSegmentedCloudInfo 记录时间
       标志位  newSegmentedCloudInfo置true
       segInfo 记录分割信息
   */
    void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr &msgIn) {
   
        timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
        segInfo = *msgIn;
        newSegmentedCloudInfo = true;
    }

2.3 outlierCloudHandler

订阅 "/outlier_cloud"的回调函数
pcl::fromROSMsg(*msgIn, *outlierCloud);
newOutlierCloud = true;
作用:为runFeatureAssociation()做准备

 /* 主题"/outlier_cloud"的回调函数,
       outlierCloud 记录传回的点云
       同时 timeNewOutlierCloud 记录header的时间
       标志位 newOutlierCloud 置为1
   */
    void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) {
   

        timeNewOutlierCloud = msgIn->header.stamp.toSec();

        outlierCloud->clear();
        pcl::fromROSMsg(*msgIn, *outlierCloud);

        newOutlierCloud = true;
    }

2.4 imuHandler

函数的实现：

1. 通过接收到的imuIn里面的四元数得到roll,pitch,yaw三个角；
2. 对加速度进行坐标变换，进行加速度坐标交换时将重力加速度去除，然后再进行$x$到$z$,$y$到$x$,$z$到$y$的变换。
3. 将欧拉角，加速度，速度保存到循环队列中；
4. 对速度，角速度，加速度进行积分，得到位移，角度和速度(AccumulateIMUShiftAndRotation())；

    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
    {
   
        //通过接收到的imuIn里面的四元数得到roll,pitch,yaw三个角
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

        // 加速度去除重力影响，同时坐标轴进行变换
        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

        //将欧拉角，加速度，速度保存到循环队列中
        imuPointerLast = (imuPointerLast + 1) % imuQueLength;

        imuTime[imuPointerLast] = imuIn->header.stamp.toSec();

        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
        imuYaw[imuPointerLast] = yaw;

        imuAccX[imuPointerLast] = accX;
        imuAccY[imuPointerLast] = accY;
        imuAccZ[imuPointerLast] = accZ;

        imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
        imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
        imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;

        //对速度，角速度，加速度进行积分，得到位移，角度和速度
        AccumulateIMUShiftAndRotation();
    }

3.特征提取

3.1 adjustDistortion

void adjustDistortion()将点云数据进行坐标变换，进行插补等工作。

1.对每个点进行处理，进行坐标轴变换。

point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;

2.针对每个点计算偏航角yaw，然后根据不同的偏航角，可以知道激光雷达扫过的位置有没有超过一半，计算的时候有一部分需要注意一下：
函数原型：

// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI)，在2PI附近
// 下面过程的主要作用是调整ori大小，满足start<ori<end
float ori = -atan2(point.x, point.z);

这里分为4种情况：

• 没有转过一半，但是start-ori>M_PI/2
• 没有转过一半，但是ori-start>3/2*M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]）
• 转过一半，end-ori>3/2*PI,ori太小
• 转过一半，ori-end>M_PI/2,太大

3.然后进行imu数据与激光数据的时间轴对齐操作。
对齐时候有两种情况，一种不能用插补来优化，一种可以通过插补进行优化。

• 不能通过插补进行优化：imu数据比激光数据早，但是没有更后面的数据(打个比方,激光在9点时出现，imu现在只有8点的)
  这种情况下while循环是以imuPointerFront == imuPointerLast结束的：

// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
   
    if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
   
        break;
    }
     imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}

• 可以通过插补来进行数据的优化：
  这种情况只有在imu数据充足的情况下才会发生，
  进行插补时，当前timeScanCur + pointTime < imuTime[imuPointerFront]，
  而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。
  imuPointerBack是imuPointerFront的前一个imu数据指针。
  插补的代码：

imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}

通过上面计算的ratioFront以及ratioBack进行插补,
因为imuRollCur，和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过2π的情况,
imuYaw转的角度比较大，需要考虑超过2π的情况。

imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
   
     imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}

后面再进行imu的速度插补与位置插补。

这一块对IMU的处理非常巧妙，在每次处理点云时，需要遍历每一个点，把第一个点作为初始坐标，随后的点均以第一个点为参考做插值计算。

                if (i == 0) {
   
                    // 此处更新过的角度值主要用在updateImuRollPitchYawStartSinCos()中,
                    // 更新每个角的正余弦值
                    imuRollStart = imuRollCur;
                    imuPitchStart = imuPitchCur;
                    imuYawStart = imuYawCur;

                    imuVeloXStart = imuVeloXCur;
                    imuVeloYStart = imuVeloYCur;
                    imuVeloZStart = imuVeloZCur;

                    imuShiftXStart = imuShiftXCur;
                    imuShiftYStart = imuShiftYCur;
                    imuShiftZStart = imuShiftZCur;

                    if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
   
                        // 该条件内imu数据比激光数据早，但是没有更后面的数据
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                    } else {
   
                        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                        float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                           / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                          / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront +
                                                 imuAngularRotationX[imuPointerBack] * ratioBack;
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront +
                                                 imuAngularRotationY[imuPointerBack] * ratioBack;
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront +
                                                 imuAngularRotationZ[imuPointerBack] * ratioBack;
                    }
                    // 在imu数据充足的情况下可以进行插补
                    imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
                    imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
                    imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

                    imuAngularRotationXLast = imuAngularRotationXCur;
                    imuAngularRotationYLast = imuAngularRotationYCur;
                    imuAngularRotationZLast = imuAngularRotationZCur;

                    // 这里更新的是i=0时刻的rpy角，后面将速度坐标投影过来会用到i=0时刻的值
                    updateImuRollPitchYawStartSinCos();
                }
                //如果不是第一个点
                else {
   
                    //把速度与位移转换回IMU的初始坐标系下
                    VeloToStartIMU();
                    TransformToStartIMU(&point);
                }

3.2 calculateSmoothness

void calculateSmoothness()用于计算光滑性，这里的计算没有完全按照LOAM论文中的公式进行。
此处的公式计算中没有除以总点数 $i$及$r$[$i$] .

    // 计算光滑性，这里的计算没有完全按照公式进行，
    // 缺少除以总点数i和r[i]
    void calculateSmoothness()
    {
   
        int cloudSize = segmentedCloud->points.size();
        for (int i = 5; i < cloudSize - 5; i++) {
   

            float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
                            + segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
                            + segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
                            + segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
                            + segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
                            + segInfo.segmentedCloudRange[i+5];            

            cloudCurvature[i] = diffRange*diffRange;

            // 在markOccludedPoints()函数中对该参数进行重新修改
            cloudNeighborPicked[i] = 0;
			// 在extractFeatures()函数中会对标签进行修改，
			// 初始化为0，surfPointsFlat标记为-1，surfPointsLessFlatScan为不大于0的标签
			// cornerPointsSharp标记为2，cornerPointsLessSharp标记为1
            cloudLabel[i] = 0;

            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }

3.3 markOccludedPoints

void markOccludedPoints()作用是将被遮挡物体的择取特征点的标志位置为1(不选为特征点),防止由于遮挡导致在被遮挡的平面上选出角特征点。

 void markOccludedPoints() {
   
        int cloudSize = segmentedCloud->points.size();

        for (int i = 5; i < cloudSize - 6; ++i) {
   

            float depth1 = segInfo.segmentedCloudRange[i];
            float depth2 = segInfo.segmentedCloudRange[i + 1];
            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] - segInfo.segmentedCloudColInd[i]));

            if (columnDiff < 10) {
   

                // 选择距离较远的那些点，并将他们标记为1
                if (depth1 - depth2 > 0.3) {
   
                    cloudNeighborPicked[i - 5] = 1;
                    cloudNeighborPicked[i - 4] = 1;
                    cloudNeighborPicked[i - 3] = 1;
                    cloudNeighborPicked[i - 2] = 1;
                    cloudNeighborPicked[i - 1] = 1;
                    cloudNeighborPicked[i] = 1;
                } else if (depth2 - depth1 > 0.3) {
   
                    cloudNeighborPicked[i + 1] = 1;
                    cloudNeighborPicked[i + 2] = 1;
                    cloudNeighborPicked[i + 3] = 1;
                    cloudNeighborPicked[i + 4] = 1;
                    cloudNeighborPicked[i + 5] = 1;
                    cloudNeighborPicked[i + 6] = 1;
                }
            }

            float diff1 = std::abs(float(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i]));
            float diff2 = std::abs(float(segInfo.segmentedCloudRange[i + 1] - segInfo.segmentedCloudRange[i]));

            // 选择距离变化较大的点，并将他们标记为1
            if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }

3.4 extractFeatures

void extractFeatures()进行特征抽取，然后分别保存到cornerPointsSharp等队列中去。
保存到不同的队列是不同类型的点云，进行了标记的工作，这一步中减少了点云数量，使计算量减少。
函数首先清空了cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat
然后对cloudSmoothness队列中sp到ep之间的点的平滑数据进行从小到大的排列。
然后按照不同的要求，将点的索引放到不同的队列中去。
另外还对点进行了标记。
最后，因为点云太多时，计算量过大，因此需要对点云进行下采样，减少计算量。

void extractFeatures(){
   
    cornerPointsSharp->clear();
    cornerPointsLessSharp->clear();
    surfPointsFlat->clear();
    surfPointsLessFlat->clear();

    for (int i = 0; i < N_SCAN; i++) {
   

        surfPointsLessFlatScan->clear();

        for (int j = 0; j < 6; j++) {
   

            // sp和ep的含义是什么???startPointer,endPointer?
            int sp = (segInfo.startRingIndex[i] * (6 - j)    + segInfo.endRingIndex[i] * j) / 6;
            int ep = (segInfo.startRingIndex[i] * (5 - j)    + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

            if (sp >= ep)
                continue;

            // 按照cloudSmoothness.value从小到大排序
            std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--) {
   
                // 每次ind的值就是等于k??? 有什么意义?
                // 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
                int ind = cloudSmoothness[k].ind;
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > edgeThreshold &&
                    segInfo.segmentedCloudGroundFlag[ind] == false) {
   
                
                    largestPickedNum++;
                    if (largestPickedNum <= 2) {
   
                        // 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，
                        // 所以这边只要选择最后两个放进队列即可
                        // cornerPointsSharp标记为2
                        cloudLabel[ind] = 2;
                        cornerPointsSharp->push_back(segmentedCloud->points[ind]);
                        cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                    } else if (largestPickedNum <= 20) {
   
						// 塞20个点到cornerPointsLessSharp中去
						// cornerPointsLessSharp标记为1
                        cloudLabel[ind] = 1;
                        cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                    } else {
   
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++) {
   
                        // 从ind+l开始后面5个点，每个点index之间的差值，
                        // 确保columnDiff<=10,然后标记为我们需要的点
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--) {
   
						// 从ind+l开始前面五个点，计算差值然后标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++) {
   
                int ind = cloudSmoothness[k].ind;
                // 平面点只从地面点中进行选择???为什么要这样做???
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < surfThreshold &&
                    segInfo.segmentedCloudGroundFlag[ind] == true) {
   

                    cloudLabel[ind] = -