激光雷达去畸变

提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档

激光雷达去畸变

  • 前言
  • 一、作业要求
  • 二、求解思路


前言

    这是视觉工坊的苏赟老师的LVI-SAM下的课程作业讲解记录,方便以后复习(如果会复习的话哈哈哈)。仅供学习交流,侵删。作业是老师改的代码框架然后挖空的形式,挺用心的,有需要作业说明和代码包的评论下我看到了会私发bdu云链接,就不在这里发出来了,感觉不太好。


一、作业要求

    基于所给的 SLAM 代码框架,实现利用里程计进行激光点云运动畸变去除的子模块,改善建图效果。只需要完善 scanRegistration.cpp 文件中的 RemoveLidarDistortion 函数,在start 与 end 之间添加代码,完成点云运动畸变去除函数,利用里程计结果对每个点进行校正。

二、求解思路

    看到输入有:cloud_in,cloud_out,dqlc,dtlc,开辟了堆区的空间用引用的方式使得输入cloud_in的点云进行畸变矫正并输出cloud_out。dqlc是四元数形式,dtlc是旋转矩阵形式。后面的调用中,可以看到实际的输入是laserCloudFilter, laserCloudUndis, dq_odom, dt_odom, 找到dq_odom, dt_odom对应的定义:


dq_odom = q_odom_curr.conjugate() * q_odom_last;
dt_odom = q_odom_curr.conjugate() * (t_odom_last - t_odom_curr);

    可以发现增量dq_odom是q_odom_curr.conjugate()即运动当前时刻的逆乘以上一时刻,以终点为参考点,计算与上一时刻的运动变换,即qwj->qjw×qwi=qji,j是当前时刻,i是上一时刻,所以是向前指的,这个相对关系很重要,即目标是将当前激光点云转到扫描终点坐标系下,扫描终点即当前扫描时刻作为原点,这里是作者怕我们弄错贴心的写出来了,不然是要写在去畸变里面的。

void RemoveLidarDistortion(const pcl::PointCloud<PointType>::Ptr &cloud_in, pcl::PointCloud<PointType>::Ptr &cloud_out, const Eigen::Quaterniond &dqlc, const Eigen::Vector3d &dtlc)
{
  // 作业:
  // TODO: 完成点云运动畸变去除函数,利用里程计结果对每个点进行校正;
  // start:

  *cloud_out = *cloud_in;

  // end.
}

//后面的调用函数
RemoveLidarDistortion(laserCloudFilter, laserCloudUndis, dq_odom, dt_odom);

    有了当前的点云和运动之后,开始去畸变:要计算当前激光点云中每一个点相对于扫描终点的时间比例,也就是雷达扫描的角度比例。注意这里的激光雷达扫描是顺时针旋转的,而用的坐标系是右手定则,yaw角是逆时针旋转的,所以角度要加负号;第一个点对应起始角度。
最终代码如下:核心思想就是知道起始、终止时刻,并用当前扫描时间求比例插值。
注意点:
    1、注意旋转四元数的方向。
    2、注意防止起始时刻越过终止时刻等此类的操作。

void RemoveLidarDistortion(const pcl::PointCloud<PointType>::Ptr &cloud_in, pcl::PointCloud<PointType>::Ptr &cloud_out, const Eigen::Quaterniond &dqlc, const Eigen::Vector3d &dtlc)
{
  // 作业:
  // TODO: 完成点云运动畸变去除函数,利用里程计结果对每个点进行校正;
  // start:
  int cloudSize = cloud_in->size();
  float startOri = -atan2(cloud_in->points[0].y,cloud_in->points[0].x);
  float endOri = -atan2(cloud_in->points[cloudSize-1].y,cloud_in->points[cloud_in->points-1].x);
  if(endOri - startOri > 3 * M_PI)
  {
    endOri -= 2 * M_PI;
  }
  else if (endOri - startOri < M_PI)
  {
    endOri += 2 * M_PI;
  }

  bool halfPassed = false;
  PointType p_in , p_out;
  Eigen::Vector3d startP , endP;
  size_t j = 0;//typedef unsigned long long size_t
  //开始遍历咯
  for (int i=0 ; i < cloudSize ; i++)
  {
    p_in = cloud_in->point[i];
    float ori = -atan2(p_in.y , p_in.x);
  //也是防止过头了这一系列奇奇怪怪的东西
    if (!halfPassed)
    {
      if (ori < startOri - M_PI/2)
      {
        ori += 2 * M_PI;
      }
      else if (ori > startOri + M_PI * 3 / 2)
      (
        ori -= 2 * M_PI;
      )

      if (ori - startOri > M_PI)
      {
        halfPassed = true;
      }
    }
    else
    {
      ori += 2 * M_PI;
      if (ori < endOri - M_PI * 3 / 2)
      {
        ori += 2 * M_PI;
      }
      else if (ori > endOri + M_PI / 2)
      {
        ori -= 2 * M_PI;
      }
    }
    if (ori > endOri)
    {
      ori -= 2 * M_PI;
    }
    
  }
  
  //算比例啦
  float ratio = (ori - startOri) / (endOri - startOri);
  //因为是换到终点,所以要1-
  float s = 1.0 - ratio;
  //Eigen::Quaterniond::Identity().slerp(t, q_last_curr) 能够实现四元数球面插值。
  //t ∈ [ 0 , 1 ] t \in [0,1]t∈[0,1]为插值点。q_last_curr为两帧之间的旋转四元数,即在两帧之间的旋转线性插入旋转四元数。
  Eigen::Quaterniond delta_qlc = Eigen::Quaterniond::Identity().slerp(s,dqlc).normalized();
  const Eigen::Vector3d delta_Plc = s * dtlc;//平移直接乘以比例就好
  endP = delta_qlc * Eigen::Vector3d(p_in.x,p_in.y,p_in.z) + delta_Plc;

  p_out = p_in;
  p_out.x = endP.x();
  p_out.y = endP.y();
  p_out.z = endP.z();
  j++;
  cloud_out->push_back(p_out);

  // end.
}

你可能感兴趣的:(SLAM学习,算法,计算机视觉)