激光雷达去畸变

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

---

前言

    这是视觉工坊的苏赟老师的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.
}