Loading [MathJax]/jax/output/CommonHTML/config.js
前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
圈层
工具
发布
首页
学习
活动
专区
圈层
工具
社区首页 >专栏 >LegoLoam(2)特征提取

LegoLoam(2)特征提取

原创
作者头像
大阳的冒险岛
修改于 2023-02-17 13:18:15
修改于 2023-02-17 13:18:15
1.1K0
举报
文章被收录于专栏:大阳岛大阳岛

1. 概述

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

2. 特征提取

2.1 点云预处理

  • 点云数据的坐标轴进行交换,变换后的坐标轴如下图:
点云坐标系.jpg
点云坐标系.jpg
  • 点云数据计算偏航角yaw, yaw = -\arctan(point.x, point.z) (-atan2返回 x / z的反正切), 由于有负号,所以yaw角是顺时针角度,且yaw的范围为 yaw = [-\Pi, \Pi)
代码语言:txt
AI代码解释
复制
    {
        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 > M_PI)
                    halfPassed = true;
            } else {
                ori += 2 * M_PI;
                if (ori < segInfo.endOrientation - M_PI * 3 / 2)
                    // end-ori>3/2*PI,ori太小
                    ori += 2 * M_PI;
                else if (ori > segInfo.endOrientation + M_PI / 2)
                    // ori-end>M_PI/2,太大
                    ori -= 2 * M_PI;
            }
            // 用 point.intensity 来保存时间
            float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
            point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;
        }

2.2 点云去畸变

  • 遍历每个点云,根据点云的周期时间,计算每个点云的时间戳
  • 寻找对应的imu数据,由于imu数据和点云数据时间戳不对齐,因此对imu数据的三个角度进行插值计算出点云时间戳下的imu的三个姿态角,特别的对于起始时刻的点云数据对应的imu三个姿态角保存
代码语言:txt
AI代码解释
复制
for (int i = 0; i < 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 < 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] > 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速度插补
                    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 > 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(&point);
                }
            }
            segmentedCloud->points[i] = point;
        }
        imuPointerLastIteration = imuPointerLast;
  • 将imu的三轴速度转到每帧点云的起始点云对应的imu坐标系
代码语言:txt
AI代码解释
复制
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;
    }
  • 点云进行去畸变,将点云根据点云生成时刻的roll、pitch和roll角转到world坐标系,然后根据上面记录的每帧点云的起始时刻的roll、pitch和roll角转到每帧点云的起始坐标系;(注意两次变换的顺序,roll->pitch->yaw, -yaw->-ptich->-roll)
代码语言:txt
AI代码解释
复制
    // 该函数的功能是把点云坐标变换到初始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->x - sin(imuRollCur) * p->y;
        float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
        float z1 = p->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-->世界坐标系-->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->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;
        p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;
        p->z = z5 + imuShiftFromStartZCur;
    }

2.3 点云特征提取

  • 云计算曲率, d = (\sum_{j\in S, i\neq j}^{} {r_i - r_j})^2
  • 去除遮挡点,过比较两点的距离差值,标记距离远的一侧附近点(即A点),车辆运动过程中,这类点很容易被前面的B点遮挡住,所以A点需要标记剔除
代码语言:txt
AI代码解释
复制
    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(segInfo.segmentedCloudRange[i - 1] - segInfo.segmentedCloudRange[i]);
            float diff2 = std::abs(segInfo.segmentedCloudRange[i + 1] - segInfo.segmentedCloudRange[i]);
            // 选择距离变化较大的点,并将他们标记为1
            if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }
  • 特征提取,每条扫描线的特征点,分成6段,每段找出2个曲率最大的非地面点特征作为cornerPointsSharp,和20个曲率大的非地面点作为cornerPointsLessSharp;选择4个曲率最小的地面特征点作为surfPointsFlat,并且在选取的点周围标记为Picked,防止选取的点过于密集,影响odom的计算精度。
代码语言:txt
AI代码解释
复制
   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++) {
                // 这里就是将每条点云数据分成了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 >= ep)
                    continue;
                // 按照cloudSmoothness.value从小到大排序
                std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, by_value());
                int largestPickedNum = 0;
                for (int k = ep; k >= sp; 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] = -1;
                        surfPointsFlat->push_back(segmentedCloud->points[ind]);
                        // 论文中nFp=4,将4个最平的平面点放入队列中
                        smallestPickedNum++;
                        if (smallestPickedNum >= 4) {
                            break;
                        }
                        cloudNeighborPicked[ind] = 1;
                        for (int l = 1; l <= 5; l++) {
                            // 从前面往后判断是否是需要的邻接点,是的话就进行标记
                            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--) {
                            // 从后往前开始标记
                            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                            if (columnDiff > 10)
                                break;
                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                for (int k = sp; k <= ep; k++) {
                    if (cloudLabel[k] <= 0)  {
                        surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
                    }
                }
            }
            // surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大
            // 进行下采样,可以大大减少计算量
            surfPointsLessFlatScanDS->clear();
            downSizeFilter.setInputCloud(surfPointsLessFlatScan);
            downSizeFilter.filter(*surfPointsLessFlatScanDS);
            *surfPointsLessFlat += *surfPointsLessFlatScanDS;
        }
    }
  • 根据imu计算初始的位姿变换
代码语言:txt
AI代码解释
复制
    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;
        }
    }
  • 根据点云匹配计算相对前后帧的odom 点云补偿,根据imu计算到的当前帧的车辆的位姿变换,将点云补偿到当前帧起始时刻的imu坐标系下
代码语言:txt
AI代码解释
复制
      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;
    }

原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。

如有侵权,请联系 cloudcommunity@tencent.com 删除。

原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。

如有侵权,请联系 cloudcommunity@tencent.com 删除。

评论
登录后参与评论
暂无评论
推荐阅读
编辑精选文章
换一批
A-LOAM代码算法
该部分函数完成激光雷达点云数据的读取,移除无效点云,计算每条扫描线点云中每个点相对于该条扫描线起始点的时间间隔(用于后续点云去几遍),根据俯仰角判断点云的扫描线id, 并根据周围点的坐标计算每个点的曲率,根据曲率将所有的点云分为sharp点,lesssharp点,flat点以及lessflat点,最后将四类点打包发送到odometry模块
大阳的冒险岛
2023/01/02
1.2K0
A-LOAM代码算法
LegoLoam(1)imu输入预处理
Lego-Loam的通信框架和Aloam相同,都是基于ros,其各个节点的运行数据流图如下:
大阳的冒险岛
2023/01/26
1.3K0
LegoLoam(1)imu输入预处理
LOAM论文和程序代码的解读(2)
文章:LOAM: Lidar Odometry and Mapping in Real-time
点云PCL博主
2022/12/27
8340
LOAM论文和程序代码的解读(2)
ALOAM:激光雷达的运动畸变补偿代码解析
激光雷达的一帧数据是过去一周期内形成的所有数据,数据仅有一时间戳,而非某个时刻的数据,因此在这一帧时间内的激光雷达或者其载体通常会发生运动,因此,这一帧的原点不一致,会导致一些问题,这个问题就是运动畸变。
点云PCL博主
2021/11/12
2.7K0
ALOAM:激光雷达的运动畸变补偿代码解析
带你玩转 3D 检测和分割(二):核心组件分析之坐标系和 Box
我们在前文玩转 MMDetection3D (一)中介绍了整个框架的大致流程,从这篇文章开始我们将会带来 MMDetection3D 中各种核心组件的解析,而在 3D 检测中最重要的核心组件之一就是坐标系和 Box 。
OpenMMLab 官方账号
2022/04/08
2.4K0
带你玩转 3D 检测和分割(二):核心组件分析之坐标系和 Box
泊车必备 | 一文详解AVM环视自标定
AVM环视系统中相机参数通常是汽车出厂前在标定车间中进行的离线阶段标定。很多供应商还提供了不依赖于标定车间的汽车自标定方法。自标定指的是:汽车在马路上慢速行驶一段路,利用车道线等先验信息标定出相机的外参。
3D视觉工坊
2023/04/30
3.2K0
泊车必备 | 一文详解AVM环视自标定
MPU6050姿态解算2-欧拉角&旋转矩阵
注:本篇中的一些图采用横线放置,若观看不方便,可点击文章末尾的阅读原文跳转到网页版
xxpcb
2020/08/26
3.5K0
MPU6050姿态解算2-欧拉角&旋转矩阵
ARKit多机画面同步解决方案,原理分析,技术讲解
由于所有玩家的坐标系的y轴都是和水平面垂直的,所以我们看做坐标系的位置相对标定点的位置,是有沿着y轴旋转了一个角度,然后平移一个值所得,只要计算出两个坐标系之间相对旋转了多少度,平移了多少增量,只要将物体坐标,也按照这个规律,旋转+平移,就可以计算物体在其他玩家坐标系中的位置
酷走天涯
2018/09/14
8750
ARKit多机画面同步解决方案,原理分析,技术讲解
从零开始学习自动驾驶系统(八)-基础知识之车辆姿态表达
辆位置和姿态是自动驾驶中的一个基础问题,只有解决了车辆的位置和姿态,才能将自动驾驶的各个模块关联起来。车辆的位置和姿态一般由自动驾驶的定位模块输出。
YoungTimes
2022/04/28
2.9K0
从零开始学习自动驾驶系统(八)-基础知识之车辆姿态表达
【前端er入门Shader系列】04—MVP矩阵与纹理映射
我们生活在三维的由各种色彩构成的大千世界里,包含了复杂的细节纹理,而纹理采样作为重要的图形学技术,能够在渲染物体表面时,使用一张图片来提供颜色信息,从而增强物体的细节和真实感。本章将结合少量数学知识,介绍如何使用MVP矩阵将三维场景投射到二维屏幕,最终执行纹理采样的过程。
CS逍遥剑仙
2025/01/13
2910
一起做激光SLAM:常见SLAM技巧使用效果对比,后端
搭建一套700行代码的激光SLAM。通过对ALOAM进行修改实验,确定对激光SLAM最核心的技巧,并接上节里程计,完成后端,构建较大场景(轨迹约2km)地图。
3D视觉工坊
2022/03/11
9880
一起做激光SLAM:常见SLAM技巧使用效果对比,后端
LVI:激光雷达子系统的特征提取梳理
上一篇文章详细分析了imageProjection节点,该节点订阅了原始点云话题、imu原始测量话题、以及VIS的里程计话题,发布了预处理(过滤无效点、有序化、去畸变)之后的点云话题cloud_deskewed和cloud_info(其中cloud_deskewed话题是普通的PointCloud2的消息类型,cloud_info话题是自定义格式的消息类型)。
3D视觉工坊
2022/03/11
6050
LVI:激光雷达子系统的特征提取梳理
粒子滤波简介
用户1147754
2018/01/05
2.5K0
粒子滤波简介
tof相机简介及三维坐标转化,plotly画3D点云[通俗易懂]
最近在做TOF相机相关的软件,近年来tof相机开始在手机,车载设备,VR等应用开始增多,产业也开始量化,是一个不错的3维相机的方向。
全栈程序员站长
2022/09/01
1.9K0
tof相机简介及三维坐标转化,plotly画3D点云[通俗易懂]
SLAM学习笔记(十九)开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中。
全栈程序员站长
2022/09/25
6.1K0
SLAM学习笔记(十九)开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
PCL深度图像(1)
目前深度图像的获取方法有激光雷达深度成像法,计算机立体视觉成像,坐标测量机法,莫尔条纹法,结构光法等等,针对深度图像的研究重点主要集中在以下几个方面,深度图像的分割技术 ,深度图像的边缘检测技术 ,基于不同视点的多幅深度图像的配准技术,基于深度数据的三维重建技术,基于三维深度图像的三维目标识别技术,深度图像的多分辨率建模和几何压缩技术等等,在PCL 中深度图像与点云最主要的区别在于 其近邻的检索方式的不同,并且可以互相转换。
点云PCL博主
2019/07/31
1.2K0
PCL深度图像(1)
【教程】详解相机模型与坐标转换
由于复制过来,如果有格式问题,推荐大家直接去我原网站上查看: 相机模型与坐标转换 - 生活大爆炸
小锋学长生活大爆炸
2024/05/25
7910
【教程】详解相机模型与坐标转换
基于全景图像与激光点云配准的彩色点云生成算法(2014年文章)
标题:The algorithm to generate color point-cloud with the registration between panoramic imageand laser point-cloud
点云PCL博主
2022/02/10
1.5K0
基于全景图像与激光点云配准的彩色点云生成算法(2014年文章)
可视化导学-相关数学知识
HTML 采用的是窗口坐标系,以参考对象(参考对象通常是最接近图形元素的 position 非 static 的元素)的元素盒子左上角为坐标原点,x 轴向右,y 轴向下,坐标值对应像素值。
Cellinlab
2023/05/17
6200
可视化导学-相关数学知识
聊一聊全景图
该文讲述了如何利用Three.js和WebRTC将全景图转换为立方体全景图,并介绍了相关的实现原理和代码示例。
聊high云
2017/10/26
3.6K0
聊一聊全景图
推荐阅读
相关推荐
A-LOAM代码算法
更多 >
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档