LegoLoam(2)特征提取

发布于

1. 概述

本节主要讲节LeogLoam中点云特征提取部分

2. 特征提取

2.1 点云预处理

  • 点云数据的坐标轴进行交换，变换后的坐标轴如下图:
点云坐标系.jpg
  • 点云数据计算偏航角yaw, yaw = -\arctan(point.x, point.z) (-atan2返回 x / z的反正切), 由于有负号，所以yaw角是顺时针角度，且yaw的范围为 yaw = [-\Pi, \Pi) ，
代码语言：txt
复制
    {
        bool halfPassed = false;
        int cloudSize = segmentedCloud->points.size();
        PointType point;
        for (int i = 0; i < cloudSize; i++) {
            // 这里xyz与laboshin_loam代码中的一样经过坐标轴变换
            // imuhandler() 中相同的坐标变换
            point.x = segmentedCloud->points[i].y;
            point.y = segmentedCloud->points[i].z;
            point.z = segmentedCloud->points[i].x;
            // -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);
            if (!halfPassed) {
                if (ori < segInfo.startOrientation - M_PI / 2)
                    // start-ori>M_PI/2，说明ori小于start，不合理，
                    // 正常情况在前半圈的话，ori-stat范围[0,M_PI]
                    ori += 2 * M_PI;
                else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
                    // ori-start>3/2*M_PI,说明ori太大，不合理
                    ori -= 2 * M_PI;

            if (ori - segInfo.startOrientation &gt; M_PI)
                halfPassed = true;
        } else {
            ori += 2 * M_PI;
            if (ori &lt; segInfo.endOrientation - M_PI * 3 / 2)
                // end-ori&gt;3/2*PI,ori太小
                ori += 2 * M_PI;
            else if (ori &gt; segInfo.endOrientation + M_PI / 2)
                // ori-end&gt;M_PI/2,太大
                ori -= 2 * M_PI;
        }
        // 用 point.intensity 来保存时间
        float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
        point.intensity = int(segmentedCloud-&gt;points[i].intensity) + scanPeriod * relTime;
    }</code></pre></div></div><h3 id="6474f" name="2.2-%E7%82%B9%E4%BA%91%E5%8E%BB%E7%95%B8%E5%8F%98">2.2 点云去畸变</h3><ul class="ul-level-0"><li>遍历每个点云，根据点云的周期时间，计算每个点云的时间戳</li><li>寻找对应的imu数据，由于imu数据和点云数据时间戳不对齐，因此对imu数据的三个角度进行插值计算出点云时间戳下的imu的三个姿态角，特别的对于起始时刻的点云数据对应的imu三个姿态角保存</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">for (int i = 0; i &lt; cloudSize; i++) {



// 2.1中点云数据预处理

if (imuPointerLast >= 0)  {

float pointTime = relTime * scanPeriod;

imuPointerFront = imuPointerLastIteration;

// while循环内进行时间轴对齐

while (imuPointerFront != imuPointerLast) {

if (timeScanCur + pointTime < imuTime[imuPointerFront]) {

break;

}

imuPointerFront = (imuPointerFront + 1) % imuQueLength;

}

if (timeScanCur + pointTime > imuTime[imuPointerFront]) {

// 该条件内imu数据比激光数据早，但是没有更后面的数据

// (打个比方,激光在9点时出现，imu现在只有8点的)

// 这种情况上面while循环是以imuPointerFront == imuPointerLast结束的

imuRollCur = imuRoll[imuPointerFront];

imuPitchCur = imuPitch[imuPointerFront];

imuYawCur = imuYaw[imuPointerFront];


                imuVeloXCur = imuVeloX[imuPointerFront];
                imuVeloYCur = imuVeloY[imuPointerFront];
                imuVeloZCur = imuVeloZ[imuPointerFront];

                imuShiftXCur = imuShiftX[imuPointerFront];
                imuShiftYCur = imuShiftY[imuPointerFront];
                imuShiftZCur = imuShiftZ[imuPointerFront];
            } else {
                // 在imu数据充足的情况下可以进行插补
                // 当前timeScanCur + pointTime &lt; imuTime[imuPointerFront]，
                // 而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针
                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]);
                // 通过上面计算的ratioFront以及ratioBack进行插补
                // 因为imuRollCur和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过2M_PI的情况
                // imuYaw转的角度比较大，需要考虑超过2*M_PI的情况
                imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
                imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
                if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] &gt; M_PI) {
                    imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
                } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] &lt; -M_PI) {
                    imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
                } else {
                    imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
                }
                // imu速度插补
                imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
                imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
                imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
                // imu位置插补
                imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
                imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
                imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
            }
            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 &gt; imuTime[imuPointerFront]) {
                    // 该条件内imu数据比激光数据早，但是没有更后面的数据
                    imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                    imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                    imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                } else {
                    // 在imu数据充足的情况下可以进行插补
                    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;
                }
                // 距离上一次插补，旋转过的角度变化值
                imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
                imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
                imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

                imuAngularRotationXLast = imuAngularRotationXCur;
                imuAngularRotationYLast = imuAngularRotationYCur;
                imuAngularRotationZLast = imuAngularRotationZCur;
                // 这里更新的是i=0时刻的rpy角，后面将速度坐标投影过来会用到i=0时刻的值
                updateImuRollPitchYawStartSinCos();
            } else {
                // 速度投影到初始i=0时刻
                VeloToStartIMU();
                // 将点的坐标变换到初始i=0时刻
                TransformToStartIMU(&amp;point);
            }
        }
        segmentedCloud-&gt;points[i] = point;
    }
    imuPointerLastIteration = imuPointerLast;</code></pre></div></div><ul class="ul-level-0"><li>将imu的三轴速度转到每帧点云的起始点云对应的imu坐标系</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">void VeloToStartIMU()
{
    // imuVeloXStart,imuVeloYStart,imuVeloZStart是点云索引i=0时刻的速度
    // 此处计算的是相对于初始时刻i=0时的相对速度，由于这里的速度加速度是world坐标系
    // 因此，相对速度在world坐标系下, world坐标系是z前，y上，x左的坐标系
    imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
    imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
    imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;

    // 现在将world坐标系转换到start坐标系，roll,pitch,yaw 取负值
    // 首先绕y轴进行(-yaw)的角度
    //    |cosry   0   sinry|
    // Ry=|0       1       0|.
    //    |-sinry  0   cosry|
    float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;
    float y1 = imuVeloFromStartYCur;
    float z1 = sinImuYawStart * imuVeloFromStartXCur + cosImuYawStart * imuVeloFromStartZCur;

    // 绕当前x轴旋转(-pitch)的角度
    //    |1     0        0|
    // Rx=|0   cosrx -sinrx|
    //    |0   sinrx  cosrx|
    float x2 = x1;
    float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;
    float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;

    // 绕当前z轴旋转(-roll)的角度
    //     |cosrz  -sinrz  0|
    //  Rz=|sinrz  cosrz   0|
    //     |0       0      1|
    imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;
    imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;
    imuVeloFromStartZCur = z2;
}</code></pre></div></div><ul class="ul-level-0"><li>点云进行去畸变，将点云根据点云生成时刻的roll、pitch和roll角转到world坐标系，然后根据上面记录的每帧点云的起始时刻的roll、pitch和roll角转到每帧点云的起始坐标系；(注意两次变换的顺序，roll-&gt;pitch-&gt;yaw,  -yaw-&gt;-ptich-&gt;-roll)</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">    // 该函数的功能是把点云坐标变换到初始imu时刻
void TransformToStartIMU(PointType *p)
{
    // 因为在adjustDistortion函数中有对xyz的坐标进行交换的过程
    // 交换的过程是x=原来的y，y=原来的z，z=原来的x
    // 所以下面其实是绕Z轴(原先的x轴)旋转，对应的是roll角
    //
    //     |cosrz  -sinrz  0|
    //  Rz=|sinrz  cosrz   0|
    //     |0       0      1|
    // [x1,y1,z1]^T=Rz*[x,y,z]
    //
    // 因为在imuHandler中进行过坐标变换，
    // 所以下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
    float x1 = cos(imuRollCur) * p-&gt;x - sin(imuRollCur) * p-&gt;y;
    float y1 = sin(imuRollCur) * p-&gt;x + cos(imuRollCur) * p-&gt;y;
    float z1 = p-&gt;z;

    // 绕X轴(原先的y轴)旋转
    //
    // [x2,y2,z2]^T=Rx*[x1,y1,z1]
    //    |1     0        0|
    // Rx=|0   cosrx -sinrx|
    //    |0   sinrx  cosrx|
    float x2 = x1;
    float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
    float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;

    // 最后再绕Y轴(原先的Z轴)旋转
    //    |cosry   0   sinry|
    // Ry=|0       1       0|
    //    |-sinry  0   cosry|
    float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
    float y3 = y2;
    float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;

    // 下面部分的代码功能是从imu坐标的原点变换到i=0时imu的初始时刻(从世界坐标系变换到start坐标系)
    // 变换方式和函数VeloToStartIMU()中的类似
    // 变换顺序：Cur--&gt;世界坐标系--&gt;Start，这两次变换中，
    float x4 = cosImuYawStart * x3 - sinImuYawStart * z3;
    float y4 = y3;
    float z4 = sinImuYawStart * x3 + cosImuYawStart * z3;

    float x5 = x4;
    float y5 = cosImuPitchStart * y4 + sinImuPitchStart * z4;
    float z5 = -sinImuPitchStart * y4 + cosImuPitchStart * z4;

    // 绕z轴(原先的x轴)变换角度到初始imu时刻，另外需要加上imu的位移漂移
    // 后面加上的 imuShiftFromStart.. 表示从start时刻到cur时刻的漂移，
    // (imuShiftFromStart.. 在start坐标系下)
    p-&gt;x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;
    p-&gt;y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;
    p-&gt;z = z5 + imuShiftFromStartZCur;
}</code></pre></div></div><h3 id="8bsag" name="2.3-%E7%82%B9%E4%BA%91%E7%89%B9%E5%BE%81%E6%8F%90%E5%8F%96">2.3 点云特征提取</h3><ul class="ul-level-0"><li>点云计算曲率，<span> d = (\sum_{j\in S, i\neq j}^{} {r_i - r_j})^2 </span>   </li><li>去除遮挡点，过比较两点的距离差值，标记距离远的一侧附近点（即A点），车辆运动过程中，这类点很容易被前面的B点遮挡住，所以A点需要标记剔除</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">    void markOccludedPoints() {
    int cloudSize = segmentedCloud-&gt;points.size();
    for (int i = 5; i &lt; 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 &lt; 10) {
            // 选择距离较远的那些点，并将他们标记为1
            if (depth1 - depth2 &gt; 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 &gt; 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(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i]);
        float diff2 = std::abs(segInfo.segmentedCloudRange[i + 1] - segInfo.segmentedCloudRange[i]);
        // 选择距离变化较大的点，并将他们标记为1
        if (diff1 &gt; 0.02 * segInfo.segmentedCloudRange[i] &amp;&amp; diff2 &gt; 0.02 * segInfo.segmentedCloudRange[i])
            cloudNeighborPicked[i] = 1;
    }
}</code></pre></div></div><ul class="ul-level-0"><li>特征提取，每条扫描线的特征点，分成6段，每段找出2个曲率最大的非地面点特征作为cornerPointsSharp，和20个曲率大的非地面点作为cornerPointsLessSharp；选择4个曲率最小的地面特征点作为surfPointsFlat，并且在选取的点周围标记为Picked，防止选取的点过于密集，影响odom的计算精度。</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">   void extractFeatures() {
    cornerPointsSharp-&gt;clear();
    cornerPointsLessSharp-&gt;clear();
    surfPointsFlat-&gt;clear();
    surfPointsLessFlat-&gt;clear();
    for (int i = 0; i &lt; N_SCAN; i++) {
        surfPointsLessFlatScan-&gt;clear();
        for (int j = 0; j &lt; 6; j++) {
            // 这里就是将每条点云数据分成了6段
            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 &gt;= ep)
                continue;
            // 按照cloudSmoothness.value从小到大排序
            std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, by_value());
            int largestPickedNum = 0;
            for (int k = ep; k &gt;= sp; k--)  {
                // 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
                int ind = cloudSmoothness[k].ind;
                if (cloudNeighborPicked[ind] == 0 &amp;&amp;
                    cloudCurvature[ind] &gt; edgeThreshold &amp;&amp;
                    segInfo.segmentedCloudGroundFlag[ind] == false) {
                    largestPickedNum++;
                    if (largestPickedNum &lt;= 2) {
                        // 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，
                        // 所以这边只要选择最后两个放进队列即可
                        // cornerPointsSharp标记为2
                        cloudLabel[ind] = 2;
                        cornerPointsSharp-&gt;push_back(segmentedCloud-&gt;points[ind]);
                        cornerPointsLessSharp-&gt;push_back(segmentedCloud-&gt;points[ind]);
                    } else if (largestPickedNum &lt;= 20) {
                        // 塞20个点到cornerPointsLessSharp中去
                        // cornerPointsLessSharp标记为1
                        cloudLabel[ind] = 1;
                        cornerPointsLessSharp-&gt;push_back(segmentedCloud-&gt;points[ind]);
                    } else {
                        break;
                    }
                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l &lt;= 5; l++) {
                        // 从ind+l开始后面5个点，每个点index之间的差值，
                        // 确保columnDiff&lt;=10,然后标记为我们需要的点
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
                        if (columnDiff &gt; 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l &gt;= -5; l--) {
                        // 从ind+l开始前面五个点，计算差值然后标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                        if (columnDiff &gt; 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }
            int smallestPickedNum = 0;
            for (int k = sp; k &lt;= ep; k++) {
                int ind = cloudSmoothness[k].ind;
                // 平面点只从地面点中进行选择? 为什么要这样做?
                if (cloudNeighborPicked[ind] == 0 &amp;&amp;
                    cloudCurvature[ind] &lt; surfThreshold &amp;&amp;
                    segInfo.segmentedCloudGroundFlag[ind] == true) {
                    cloudLabel[ind] = -1;
                    surfPointsFlat-&gt;push_back(segmentedCloud-&gt;points[ind]);
                    // 论文中nFp=4，将4个最平的平面点放入队列中
                    smallestPickedNum++;
                    if (smallestPickedNum &gt;= 4) {
                        break;
                    }
                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l &lt;= 5; l++) {
                        // 从前面往后判断是否是需要的邻接点，是的话就进行标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
                        if (columnDiff &gt; 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l &gt;= -5; l--) {
                        // 从后往前开始标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                        if (columnDiff &gt; 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }
            for (int k = sp; k &lt;= ep; k++) {
                if (cloudLabel[k] &lt;= 0)  {
                    surfPointsLessFlatScan-&gt;push_back(segmentedCloud-&gt;points[k]);
                }
            }
        }
        // surfPointsLessFlatScan中有过多的点云，如果点云太多，计算量太大
        // 进行下采样，可以大大减少计算量
        surfPointsLessFlatScanDS-&gt;clear();
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.filter(*surfPointsLessFlatScanDS);
        *surfPointsLessFlat += *surfPointsLessFlatScanDS;
    }
}</code></pre></div></div><ul class="ul-level-0"><li>根据imu计算初始的位姿变换</li></ul><div class="rno-markdown-code"><div class="rno-markdown-code-toolbar"><div class="rno-markdown-code-toolbar-info"><div class="rno-markdown-code-toolbar-item is-type"><span class="is-m-hidden">代码语言：</span>txt</div></div><div class="rno-markdown-code-toolbar-opt"><div class="rno-markdown-code-toolbar-copy"><i class="icon-copy"></i><span class="is-m-hidden">复制</span></div></div></div><div class="developer-code-block"><pre class="prism-token token line-numbers language-txt"><code class="language-txt" style="margin-left:0">    void updateInitialGuess() {
    imuPitchLast = imuPitchCur;
    imuYawLast = imuYawCur;
    imuRollLast = imuRollCur;
    imuShiftFromStartX = imuShiftFromStartXCur;
    imuShiftFromStartY = imuShiftFromStartYCur;
    imuShiftFromStartZ = imuShiftFromStartZCur;
    imuVeloFromStartX = imuVeloFromStartXCur;
    imuVeloFromStartY = imuVeloFromStartYCur;
    imuVeloFromStartZ = imuVeloFromStartZCur;
    // transformCur是在Cur坐标系下的 p_start = R*p_cur+t
    // R和t是在Cur坐标系下的
    // 而imuAngularFromStart在 imu坐标系，加负号即 last扫描的角度 - cur 扫描的角度
    if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0) {
        //  imgAngularFromStartX 表示一次扫描imu旋转过的角度
        transformCur[0] = -imuAngularFromStartY;
        transformCur[1] = -imuAngularFromStartZ;
        transformCur[2] = -imuAngularFromStartX;
    }
    // 速度乘以时间，当前变换中的位移
    if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) {
        // imuVeloFromStartX 表示一次扫描imu 运动的距离, 位移向量在每次扫描的Start坐标系
        transformCur[3] -= imuVeloFromStartX * scanPeriod;
        transformCur[4] -= imuVeloFromStartY * scanPeriod;
        transformCur[5] -= imuVeloFromStartZ * scanPeriod;
    }
}</code></pre></div></div><ul class="ul-level-0"><li>根据点云匹配计算相对前后帧的odom



点云补偿，根据imu计算到的当前帧的车辆的位姿变换，将点云补偿到当前帧起始时刻的imu坐标系下
代码语言：txt
复制
      void TransformToStart(PointType const *const pi, PointType *const po) {

// intensity代表的是：整数部分ring序号，小数部分是当前点在这一圈中所花的时间

// 由于点云是10HZ,即单帧扫描是0.1ms, 那么s表示扫描到当前point的时间占整个scan的时间的比例

float s = 10 * (pi->intensity - int(pi->intensity));

float rx = s * transformCur[0];

float ry = s * transformCur[1];

float rz = s * transformCur[2];

float tx = s * transformCur[3];

float ty = s * transformCur[4];

float tz = s * transformCur[5];

// 这里rz 旋转是imu原坐标系的x轴，新坐标的z轴

// 就是这里是补偿到开始帧的imu坐标系，因为这里的transformCur 的不管是旋转还是平移，都是Start - Curr, 所以该位姿变换以后是补偿到上一帧扫描起始时刻

float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);

float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);

float z1 = (pi->z - tz);

float x2 = x1;

float y2 = cos(rx) * y1 + sin(rx) * z1;

float z2 = -sin(rx) * y1 + cos(rx) * z1

po->x = cos(ry) * x2 - sin(ry) * z2;

po->y = y2;

po->z = sin(ry) * x2 + cos(ry) * z2;

po->intensity = pi->intensity;

}
#unix