LIO-SAM框架：点云预处理前端---畸变矫正

前言
畸变矫正
result

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

其中点云运动畸变矫正的代码在图像投影的节点中

可以看到该节点订阅 3种消息：

原始点云数据
原始imu数据
imu预积分后预测的imu里程计数据

其中完成的一个主要功能就是进行畸变矫正。上一篇博客主要解读其畸变矫正数据预处理部分；本篇博客将解读其畸变矫正处理流程部分。

畸变矫正

将点云投影到一个矩阵上，并保存每个点的信息,并在内部进行畸变矫正

    void projectPointCloud(){

        int cloudSize = laserCloudIn->points.size();for (int i = 0; i < cloudSize; ++i){

遍历整个点云

            PointType thisPoint; thisPoint.x = laserCloudIn->points[i].x;thisPoint.y = laserCloudIn->points[i].y;thisPoint.z = laserCloudIn->points[i].z;thisPoint.intensity = laserCloudIn->points[i].intensity;

取出对应的某个点

float range = pointDistance(thisPoint);

计算这个点距离lidar中心的距离

            if (range < lidarMinRange || range > lidarMaxRange)continue;

距离太小或者太远都认为是异常点

            int rowIdn = laserCloudIn->points[i].ring;if (rowIdn < 0 || rowIdn >= N_SCAN)continue;if (rowIdn % downsampleRate != 0)continue;

取出对应的在第几根scan上
scan id 合理判断
如果需要降采样，就根据scan id 适当跳过

            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; static float ang_res_x = 360.0/float(Horizon_SCAN);int 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;

计算水平角
计算水平分辨率
计算水平线束id ，转换到x负方向为起始，顺时针为正方向，范围[0-H]
对水平角做补偿，因为雷达是顺时针旋转，
对水平id进行检查

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)continue;

如果这个位置有填充了就跳过
点云不是完全的360度，可能会多一些

            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

对点做运动补偿

rangeMat.at<float>(rowIdn, columnIdn) = range;

将这个点的距离数据保存进这个range矩阵种

int index = columnIdn + rowIdn * Horizon_SCAN;

算出点的索引

fullCloud->points[index] = thisPoint;

保存这个点的坐标

之后来看下运动补偿得函数deskewPoint

    PointType deskewPoint(PointType *point, double relTime){

        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)return *point;

判断是否可以进行运动补偿，不能得话则之间返回原点
判断依据：

deskewFlag 是原始点云没有 time得标签则为-1
cloudInfo.imuAvailable 的原始imu里面的数据判断

        double pointTime = timeScanCur + relTime;

relTime 是相对时间，加上起始时间就是绝对时间

        float rotXCur, rotYCur, rotZCur;findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

通过findRotation函数计算当前点相对起始点的相对旋转

其内部为：

    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur){*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

先将相对旋转至0

        int imuPointerFront = 0;while (imuPointerFront < imuPointerCur){if (pointTime < imuTime[imuPointerFront])break;++imuPointerFront;}

找到距离该点云时间最近的大于该点云时间的点

        if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0){*rotXCur = imuRotX[imuPointerFront];*rotYCur = imuRotY[imuPointerFront];*rotZCur = imuRotZ[imuPointerFront];}

如果时间戳不在两个imu的旋转之间，就直接赋值了

        } else {  int imuPointerBack = imuPointerFront - 1;double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;}

否则作一个线性插值，得到相对旋转
算两个权重进行插值

        float posXCur, posYCur, posZCur;findPosition(relTime, &posXCur, &posYCur, &posZCur);

这里没有计算平移补偿如果运动不快的话

        if (firstPointFlag == true){transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();firstPointFlag = false;}

计算第一个点的相对位姿

        Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);Eigen::Affine3f transBt = transStartInverse * transFinal;

计算当前点和第一点的相对位姿

        newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);newPoint.intensity = point->intensity;return newPoint;

就是R*p+t ，把点补偿到第一个点对应的时刻的位姿

然后看提取出有效的点的信息函数 cloudExtraction

    void cloudExtraction(){

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

遍历每一根scan

cloudInfo.startRingIndex[i] = count - 1 + 5;

这个scan可以计算曲率的起始点（计算曲率需要左右各五个点）

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

遍历该 scan上的每个点

                if (rangeMat.at<float>(i,j) != FLT_MAX)//FLT_MAX就是最大的浮点数{

判断该点是否是一个有效的点
rangeMat的每个点初始化为FLT_MAX ，如果点有效，则会赋值为 range

cloudInfo.pointColInd[count] = j;

点云信息里面这个点对应着哪一个垂直线

cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);

点云信息里面保存它的距离信息

 extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

他的3d坐标信息

cloudInfo.endRingIndex[i] = count -1 - 5;

这个scan可以计算曲率的终端

在上面处理完后
即可发布点云

    void publishClouds(){cloudInfo.header = cloudHeader;cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);pubLaserCloudInfo.publish(cloudInfo);}

最后将处理后的点云发布出去

result

LIO-SAM框架：点云预处理前端---畸变矫正及提取有效点云相关推荐

LIO-SAM：点云预处理前端---畸变矫正数据预处理
LIO-SAM框架:点云预处理前端---畸变矫正数据预处理前言激光雷达畸变矫正畸变矫正数据预处理总结前言 LIO-SAM的全称是:Tightly-coupled Lidar Inertial ...
html影音播放器百度云,关于前端直播（videoJS与百度云web播放器：Cyberplayer3.0试用）...
文章目录 videoJS 打开方式记录初始化居中播放按钮百度视频播放器demo 打开方式修改 videoJS videoJs 文档 https://docs.videojs.com/docs ...
PCLl从Vlp-16录制好的bag包提取点云数据
本文将记录Vlp-16的使用的一些常规操作,以及报错解决方法. 查看录制的bag包信息 rosbag info out.bag 录制的bag包为out.bag保存在data文件夹下,创建文件夹存放提取 ...
在阿里云做前端，是种怎样的体验？
阿里妹导读:前端是个资源型角色,在认知里对业务的理解深度不够,加上通常负责业务领域很广,比较难有积累和沉淀.如果问一个毕业10年的JAVA老司机,他跟你谈的一定是大流量下的分布式架构,而前端可能还是昨 ...
Node.js Web 框架再进化 - 面向前端与未来标准
大厂技术高级前端 Node进阶点击上方程序员成长指北,关注公众号回复1,加入高级Node交流群 Web 开发一直是 Node.js 的主流方向,无论新人必学的 Express / Koa, ...
在阿里云做前端这几年～
点击上方"IT平头哥联盟",选择"置顶或者星标" 一起进步- 作者:阿里云高级前端技术专家城池 https://yq.aliyun.com/articles/ ...
一文带你快速拆解云智慧前端技术架构
主讲人:王海虎,云智慧/智能研究院/算法工程经理讲师简介:6年开发经验,主攻可视化方向.3d引擎.视野分析方向.18年开始做可视化方向,从前端工程师做到开发经理:开源项目FlyFish的负责人(荣获 ...
如何挑选适合的前端框架(去哪儿网前端架构师司徒正美)
前端框架不断推新,众多IT企业都面临着"如何选择框架","是否需要再造轮子"的抉择.去哪儿网前端架构师司徒正美分析了各主流行框架优劣点.适用场景,并针对不同规模 ...
前端的date类型后台接收_腾讯高级前端工程师支招，云开发实现小程序打赏和提现云开发实践...
导语微信打赏支付和红包提现,是日常高频功能,那么基于小程序云开发,如何实现小程序的打赏支付和红包提现呢?腾讯工程师给你支招. 如何实现小程序打赏支付 1.1 小程序打赏支付功能介绍这次的打赏功能, ...
前端React项目中实现萤石云ezuikit摄像头的播放与控制
最近要在react项目中使用萤石云提供的ezuikit库来接入萤石云摄像头,实现远程播放.控制移动.放大缩小等功能,首先百度搜类似的需求,搜不到,只能自己采坑,登萤石云官网,看对应文档. 一.登录萤石 ...

LIO-SAM框架：点云预处理前端---畸变矫正及提取有效点云

LIO-SAM框架：点云预处理前端---畸变矫正

前言

畸变矫正

result

LIO-SAM框架：点云预处理前端---畸变矫正及提取有效点云相关推荐

最新文章

热门文章