0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看威廉希尔官方网站 视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架

工程师邓生 来源:古月居 作者:月照银海似蛟龙 2022-09-14 10:11 次阅读

前言

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

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

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。

实现了高精度、实时的移动机器人的轨迹估计和建图。

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

23c33d90-33ba-11ed-ba43-dac502259ad0.png

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

原始点云数据

原始imu数据

imu预积分后预测的imu里程计数据其中完成的一个主要功能就是进行畸变矫正

本篇博客将解读其畸变矫正处理流程部分。

23d60f9c-33ba-11ed-ba43-dac502259ad0.png

畸变矫正

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

  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(rowIdn, columnIdn) != FLT_MAX)        continue;

如果这个位置有填充了就跳过


点云不是完全的360度,可能会多一些

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

对点做运动补偿

rangeMat.at(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(i,j) != FLT_MAX)//FLT_MAX就是最大的浮点数        {

判断该点 是否 是一个 有效的点


rangeMat的每个点初始化为FLT_MAX ,如果点有效,则会赋值为 range

cloudInfo.pointColInd[count] = j;

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

cloudInfo.pointRange[count] = rangeMat.at(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

23ec0fe0-33ba-11ed-ba43-dac502259ad0.png

23fb2ef8-33ba-11ed-ba43-dac502259ad0.png

240d608c-33ba-11ed-ba43-dac502259ad0.png





审核编辑:刘清

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 3D
    3D
    +关注

    关注

    9

    文章

    2875

    浏览量

    107490
  • SAM
    SAM
    +关注

    关注

    0

    文章

    112

    浏览量

    33519
  • 激光雷达
    +关注

    关注

    968

    文章

    3971

    浏览量

    189855

原文标题:LIO-SAM点云预处理前端:畸变矫正

文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    科普:文了解固态和半固态激光雷达

    激光雷达(LiDAR,Laser Detecting and Ranging)作为种先进的传感威廉希尔官方网站 ,通过发射激光脉冲并测量其返回时间来计算目标距离,被广泛应用于自动驾驶、机器人、工业自动化等领域
    的头像 发表于 12-23 18:06 65次阅读

    激光雷达在SLAM算法中的应用综述

    SLAM算法运行的重要传感器。基于激光雷达的SLAM算法,对激光雷达SLAM总体框架进行介绍,详细阐述前端里程计、后端优化、回环检测、地图构建模块的作用并总结所使用的算法;按由2D到
    的头像 发表于 11-12 10:30 513次阅读
    <b class='flag-5'>激光雷达</b>在SLAM算法中的应用综述

    激光雷达的维护与故障排查技巧

    激光雷达(LiDAR,Light Detection and Ranging)是利用激光进行距离测量和目标识别的威廉希尔官方网站 。它广泛应用于无人驾驶汽车、地理信息系统(GIS)、环境监测、航
    的头像 发表于 10-27 11:04 957次阅读

    激光雷达威廉希尔官方网站 的基于深度学习的进步

    信息。这使得激光雷达在自动驾驶、无人机、机器人等领域具有广泛的应用前景。 二、深度学习威廉希尔官方网站 的发展 深度学习是机器学习的分支,它通过模拟人脑的神经网络结构来处理和分析数据。近年来,深度学习威廉希尔官方网站 在图像识别、语音
    的头像 发表于 10-27 10:57 371次阅读

    激光雷达威廉希尔官方网站 的发展趋势

    激光雷达(LiDAR,Light Detection and Ranging)威廉希尔官方网站 是种通过发射激光脉冲并接收其反射来测量距离和速度的遥感威廉希尔官方网站 。它在多个领域,如测绘、环境监测、自动驾驶汽车和无人机等
    的头像 发表于 10-27 10:44 804次阅读

    LIDAR激光雷达逆向建模能用到revit当中吗

    LIDAR激光雷达逆向建模是利用激光雷达威廉希尔官方网站 获取物体表面数据,然后通过计算机软件进行建模的方法。在建筑行业中,这种方法可以用于建筑物的三维建模、结构分析、施工模拟等。Revit是
    的头像 发表于 08-29 17:23 534次阅读

    光学雷达激光雷达的区别是什么

    光学雷达激光雷达是两种不同的遥感威廉希尔官方网站 ,它们在原理、应用、优缺点等方面都存在定的差异。以下是对光学雷达激光雷达的比较: 定义和原理 光学
    的头像 发表于 08-29 17:20 1291次阅读

    激光雷达点云数据包含哪些信息

    激光雷达(LiDAR)是利用激光威廉希尔官方网站 进行距离测量的遥感威廉希尔官方网站 。它通过发射激光脉冲并接收反射回来的光束,来测量物体与
    的头像 发表于 08-29 17:18 915次阅读

    文看懂激光雷达

        文章大纲 城市 NOA 成竞争高地,政策助力高阶智能驾驶加速落地 成本下探+智驾升级,2030年激光雷达市场规模有望超万亿       ·城市 NOA面临工况复杂问题,激光雷达为“优选
    的头像 发表于 06-27 08:42 619次阅读
    <b class='flag-5'>一</b>文看懂<b class='flag-5'>激光雷达</b>

    亮道智能:发布全新激光雷达,未来主攻固态激光雷达低价市场

    亮道智能官方透露该公司已为众多行业客户提供全面的激光雷达感知解决方案,包括车规级激光雷达硬件及其感知功能开发、测试验证及数据服务。
    的头像 发表于 03-06 15:22 720次阅读

    激光雷达LIDAR基本工作原理

    激光雷达LiDAR工作原理激光雷达LiDAR的全称为LightDetectionandRanging激光探测和测距,又称光学雷达
    的头像 发表于 03-05 08:11 5102次阅读
    <b class='flag-5'>激光雷达</b>LIDAR基本工作原理

    一个激光雷达,需要哪些基本部件?

    激光雷达(LiDAR)是激光探测及测距系统的简称,目前广泛应用在无人驾驶和扫地机器人等领域。这种广泛的应用方面得益于激光雷达的性能提升,
    发表于 01-19 14:22 1090次阅读
    做<b class='flag-5'>一个</b><b class='flag-5'>激光雷达</b>,需要哪些基本部件?

    华为激光雷达参数怎么设置

    华为激光雷达种常用的传感器威廉希尔官方网站 ,可用于距离测量和感应。它的参数设置对于确保其性能和功能至关重要。在本文中,我们将详细介绍华为激光雷达的参数设置以及其影响和应用。 首先,我们需要了解激光雷达
    的头像 发表于 01-19 14:17 1776次阅读

    禾赛科技推出AT512激光雷达

    今日,禾赛科技在智能驾驶领域再创辉煌,正式推出了业界翘首以盼的“性能王牌”——AT512激光雷达。作为禾赛AT系列的巅峰之作,AT512不仅代表了禾赛在激光雷达威廉希尔官方网站 上的突破性进展,更是智能驾驶领域的一大里程碑。
    的头像 发表于 01-09 14:32 829次阅读

    禾赛科技激光雷达累计交付突破30万台 全球首个创下此里程碑的车载激光雷达公司

    今日,全球激光雷达领导者禾赛科技宣布,公司自成立以来激光雷达累计交付量突破30 万台,成为全球首个创下此里程碑的车载激光雷达公司。   自 2022 年 7 月正式开启前装量产以来,禾
    的头像 发表于 12-27 09:44 551次阅读
    禾赛科技<b class='flag-5'>激光雷达</b>累计交付突破30万台 全球首个创下此<b class='flag-5'>里程</b>碑的车载<b class='flag-5'>激光雷达</b>公司