修改iview源码无效_LOAM源码解析1——scanRegistration

最新推荐文章于 2022-12-31 10:12:36 发布

二狗学长好

最新推荐文章于 2022-12-31 10:12:36 发布

阅读量338

点赞数

文章标签：修改iview源码无效

本文链接：https://blog.csdn.net/weixin_30851637/article/details/112785384

版权

入手激光雷达SLAM，从最经典的LOAM源码开始，之后会对秦通在此基础上改进的A-LOAM源码进行观看，应该就会很简单了，第三步是把LEGO-LOAM的源码进行解读。之前对LOAM的论文进行了详细解读。

LOAM的源码主要分为scanRegistration提取特征点、laserOdometry 10HZ估计位姿、laserMapping 1HZ构建三维地图、transforMaintenance 位姿优化等四个模块。本文主要参考LOAM_NOTED，昨天看到攻城狮说工作项目中是来不及你详细阅读代码的，从一个灵感出现到实现大概只给你一周时间，这中间会包括两三天时间阅读论文、源码，两三天在源码基础上进行修改，时间就是这么紧张。

本着不重复造轮子的原则，本文将在关键地方加上自己理解，帮助大家更加详尽了解。这里注释的是LOAM的velodyne三维激光版本，主要考虑到KITTI上有Velodyne16线的数据，可以用来测试。今天进行第一部分Point Clond Registration模板，对应的scanRegistration.cpp，主要完成的工作有：对点云和IMU数据预处理、对接收的点云数据划分到不同线中存储、特征点（边缘点+平面点）提取等。

scanRegistration.cpp

下面从main函数开始在ros中订阅了2个话题，发布了6个话题。两个主要的回调函数：laserCloudHandler、imuHandler分别处理点云和IMU数据。其中，最主要的是laserCloudHandler回调函数。

main.cpp

int main(int argc, char** argv)
{
  ros::init(argc, argv, "scanRegistration");
  ros::NodeHandle nh;
  // 两个订阅话题：点云、IMU
  // 两个主要的回调函数：laserCloudHandler、imuHandler
  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> 
                                  ("/velodyne_points", 2, laserCloudHandler);
  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
  // 6个发布话题
  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/velodyne_cloud_2", 2);
  pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>
                                        ("/laser_cloud_sharp", 2);
  pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>
                                            ("/laser_cloud_less_sharp", 2);
  pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>
                                       ("/laser_cloud_flat", 2);
  pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_less_flat", 2);
  pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);
  // ros::spin()进入循环，一直调用回调函数，用户输入Ctrl+C退出
  ros::spin();
  return 0;
}

回调函数 laserCloudHandler

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)

主要包括以下模块：

1、点云预处理：过滤无效点、计算起始和终止点的方位角 2、 根据每个点的仰角将点划入不同线中，共16线，记录线束号和获取的相对时间（也就是代码中的intensity，这个不是强度） 3、使用IMU数据插值计算点云中点的位置，消除由于非匀速运动造成的运动畸变 4、计算所有点的曲率，剔除两类点 5、 提取边缘点和平面点 6、ROS发布消息

1、点云预处理

首先过滤无效点、计算起始和终止点的方位角。计算起始、终止点的角度是为了计算点云数据中每个点在本次扫描点云数据中的相对位置，从而得到每个点获取的时间relTime，为后面IMU插值做准备。

后面会用上的变量为：

1、每次扫描点开始和结束的索引： scanStartInd、scanEndInd 2、pcl点云数据： laserCloudIn 3、起始和终止点的方位角： startOri、endOri

//------------1、点云预处理----------------//

  //丢弃前20个点云数据，确保有IMU数据 后再进行点云数据的处理
  if (!systemInited) {
    systemInitCount++;
    if (systemInitCount >= systemDelay) {
      systemInited = true;
    }
    return;
  }

  //记录每个scan有曲率的点的开始和结束索引，初始值为0·
  std::vector<int> scanStartInd(N_SCANS, 0);
  std::vector<int> scanEndInd(N_SCANS, 0);

  //点云时间戳
  double timeScanCur = laserCloudMsg->header.stamp.toSec();
  //把sensor_msg类型转换成pcl模板点云数据laserCloudIn
  pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

  //移除空点,无效值
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
  //点云点的数量
  int cloudSize = laserCloudIn.points.size();

  // 点云的方位角：起始角度/终止角度？？？
  // 扫描开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
  float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  // 扫描结束点的旋转角，加2*pi使点云旋转周期为2*pi
  float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                        laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

  //结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描
  //正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正
  // 为什么不是pi< X <2*pi ？？？？？
  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }

未理解的问题：为什么要保证起始和终止点云方位角差距都在(PI,3PI)范围，>PI如果是为了保证扫描至少180度，雷达旋转扫描范围足够，那么<3PI？？？不是一次scan 应该扫描一个圆周吗，2PI吗？那结果为什么不是(PI,2PI)，这样就可以保证扫描一个圆周了。(2PI,3PI)这180度的点可是已经扫描过了哦。

if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }

2、根据角度将点划入不同线中

遍历所有点，首先从激光雷达坐标系转移到车体坐标系，计算点的仰角，根据仰角过滤16线以外的点，并为每个点分配激光线号scanID，根据点云中点旋转角度和整个周期旋转角度的比率获得相对时间relTime，并获得点强度（限号+点相对时间）。中间，根据扫描线是否过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿。

重要的变量：

laserCloudScans：根据线号分布的点云数据 point.intensity：点的强度（线号+点相对时间）

// ---------------------2、根据角度将点划入不同线中------------//

  bool halfPassed = false;  //lidar扫描线是否旋转过半
  int count = cloudSize;
  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);// 保留不有不同特征的点云
  // 遍历点云，计算每个点的角度
  for (int i = 0; i < cloudSize; i++) {
    // 1.坐标轴交换，velodyne lidar的坐标系也转换到z轴向前，x轴向左的右手坐标系
    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

    // 2. 计算点的仰角，过滤16线以外的点
    //计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度
    float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
    int scanID;
    int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); //仰角四舍五入(加减0.5截断效果等于四舍五入)
    if (roundedAngle > 0){
      scanID = roundedAngle;
    }
    else {
      scanID = roundedAngle + (N_SCANS - 1);
    }
    //过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15]，剔除16线以外的杂点。
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      count--;
      continue;
    }

    // 3.根据扫描线是否旋转过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿
    float ori = -atan2(point.x, point.z);//该点的旋转角
    // 如果没有旋转过半
    if (!halfPassed) {
      //确保-pi/2 < ori - startOri < 3*pi/2
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }
      // 旋转过半，旋转角度超过180度
      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } 
    // 旋转过半
    else {
      ori += 2 * M_PI;// 先加2*pi

      //确保-3*pi/2 < ori - endOri < pi/2
      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      } 
    }

    //点云中点的相对时间:点旋转的角度与整个周期旋转角度的比率, -0.5 < relTime < 1.5
    float relTime = (ori - startOri) / (endOri - startOri);
    //点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号scanID，小数部分是该点的相对时间）
    //匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
    point.intensity = scanID + scanPeriod * relTime;// scanPeriod 扫描一圈0.1s
   ........
   ...第三步、IMU插值计算点云中点位置.........
   ........
   //将每个补偿矫正的点放入对应线号的容器，将点按照每一层线，分类压入16个数组中
    laserCloudScans[scanID].push_back(point);
    }
    // 更新总的点云 laserCloud
  cloudSize = count;//获得有效范围内的点的数量
  pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
  for (int i = 0; i < N_SCANS; i++) {//将所有的点按照线号从小到大放入一个容器
    *laserCloud += laserCloudScans[i];
  }

坐标轴转化：从蓝色的传感器坐标系到红色的车体坐标系。velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系，车体坐标系为z轴向前,x轴向左,y轴向上的右手坐标系

point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

在LOAM源码中对于仰角的定义为： $theta =arctan(z/sqrt{x^{2}+ y^{2}})$

float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;

未理解的问题：第三步根据扫描线是否旋转过半，选择与起始位置还是终止位置进行差值计算。

3、使用IMU数据进行插值计算点云的中点的位置，使用imu矫正点云畸变

使用IMU数据进行插值计算点云的中点的速度，位移和欧拉角，消除由于非匀速运动造成的运动畸变。首先需要寻找点云的时间戳小于IMU时间戳，点云处于imuPointerBack和imuPointerFront之间，据此进行线性插值，计算点云速度、位移、欧拉角。最后转换到sweep k的初始imu坐标系下。 插值的方式为：先根据与IMU的时间距离计算权重ratioFront、ratioBack，再根据权重计算点云。

// -----------------3、使用IMU数据进行插值计算点云的中点的位置，消除由于非匀速运动造成的运动畸变----------------//

    //点时间=点云时间+周期时间
    if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变
      float pointTime = relTime * scanPeriod;//小数点，计算点的周期时间

      //寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
      while (imuPointerFront != imuPointerLast) {
        if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
          break;
        }
        imuPointerFront = (imuPointerFront + 1) % imuQueLength;
      }

      //没找到,此时imuPointerFront==imuPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用
      if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
        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];
      } 
      //找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间
      //据此线性插值，计算点云点的速度，位移和欧拉角
      else {
        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]);

        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;
        }

        //本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFront
        imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
        imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
        imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

        imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
        imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
        imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
      }

      //如果是第一个点,记住点云起始位置的速度，位移，欧拉角
      if (i == 0) {
        imuRollStart = imuRollCur;
        imuPitchStart = imuPitchCur;
        imuYawStart = imuYawCur;

        imuVeloXStart = imuVeloXCur;
        imuVeloYStart = imuVeloYCur;
        imuVeloZStart = imuVeloZCur;

        imuShiftXStart = imuShiftXCur;
        imuShiftYStart = imuShiftYCur;
        imuShiftZStart = imuShiftZCur;
      } 
      //计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移、速度畸变，并对点云中的每个点位置信息重新补偿矫正
      // Lidar位移、速度转移到IMU起始坐标系下
      else {
        ShiftToStartIMU(pointTime);
        VeloToStartIMU();
        TransformToStartIMU(&point);
      }
    }

我们已经知道了每个点在车体坐标系的位置，我们想要的是这一帧数据中该点相对于起始点由于加减速造成的运动畸变，最后的三个环节是为了把雷达位移、速度转移到IMU起始坐标系下。从世界坐标系下加减速造成的运动畸变，将运动畸变值绕y,x,z轴旋转后得到IMU起始坐标系下的运动畸变。下面分别为对位移、速度的畸变转移到IMU起始坐标系，以及去除点云加减速产生的位移畸变。

ShiftToStartIMU(pointtime)

求当前点的位移相对于点云起始点的位移畸变，先计算全局坐标系下的然后再转换到IMU起始点的坐标系中。首先计算畸变位移，再根据rpy的反向，绕y,x,z轴分别旋转。即可将位移畸变从世界坐标系转移到局部坐标系。 rpy即roll,pitch,yaw分别是绕着z,x,y轴旋转，这里回去要反向旋转。

imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
//计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
void ShiftToStartIMU(float pointTime)
{
  //计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)
  //imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
  imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
  imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
  imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;

  /********************************************************************************
  rpy：Rz(roll).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tg
  transfrom from the global frame to the local frame
  *********************************************************************************/

  //绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
  float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
  float y1 = imuShiftFromStartYCur;
  float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;

  //绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  //绕z轴旋转(-imuRollStart)，即Rz(roll).inverse
  imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
  imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
  imuShiftFromStartZCur = z2;
}

VeloToStartIMU()

与上面类似，不同的是速度畸变通过以下公式：

imuVeloFromStartCur = imuVeloCur - imuVeloStart;
//计算局部坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变（增量）
void VeloToStartIMU()
{
  //计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)
  imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
  imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
  imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;

  /********************************************************************************
    rpy:Rz(roll).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vg
    transfrom from the global frame to the local frame
  *********************************************************************************/

  //绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
  float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
  float y1 = imuVeloFromStartYCur;
  float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;

  //绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
  float x2 = x1;
  float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
  float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;

  //绕z轴旋转(-imuRollStart)，即Rz(pitch).inverse
  imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
  imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
  imuVeloFromStartZCur = z2;
}

TransformToStartIMU(&point)

去除点云加减速产生的位移畸变。先rpy轴将点转换到世界坐标系然后再ypr由世界坐标系转换到IMU起始点坐标系，最后减去加减速造成的非匀速畸变的值。

//去除点云加减速产生的位移畸变
void TransformToStartIMU(PointType *p)
{
  /********************************************************************************
    Ry*Rx*Rz*Pl, transform point to the global frame
  *********************************************************************************/
  //绕z轴旋转(imuRollCur)
  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轴旋转(imuPitchCur)
  float x2 = x1;
  float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
  float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;

  //绕y轴旋转(imuYawCur)
  float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
  float y3 = y2;
  float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;

  /********************************************************************************
    Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pg
    transfrom global points to the local frame
  *********************************************************************************/

  //绕y轴旋转(-imuYawStart)
  float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
  float y4 = y3;
  float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;

  //绕x轴旋转(-imuPitchStart)
  float x5 = x4;
  float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
  float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;

  //绕z轴旋转(-imuRollStart)，然后叠加平移量
  p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
  p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
  p->z = z5 + imuShiftFromStartZCur;
}

4、计算所有点曲率、剔除两类点

特征点的提取，通过遍历每个点（除了前五个和后五个）差值的平方来代表曲率，计算各点曲率并找到所有线的起点终点位置；重要变量：

cloudCurvature[i]：每个点的曲率 scanStartInd[scanCount]：scan起始位置的索引（去除前5个点） scanEndInd[scanCount - 1]：scan终止位置的索引（去除后5个点）

// 1.使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过
  // 该点与周围10个点的偏差
  int scanCount = -1;
  for (int i = 5; i < cloudSize - 5; i++) {
    float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x 
                + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x 
                + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x 
                + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
                + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
                + laserCloud->points[i + 5].x;
    float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y 
                + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y 
                + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y 
                + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
                + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
                + laserCloud->points[i + 5].y;
    float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z 
                + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z 
                + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z 
                + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
                + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
                + laserCloud->points[i + 5].z;
    //曲率计算
    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;

    //记录曲率点的索引
    cloudSortInd[i] = i;
    //初始时，点全未筛选过
    cloudNeighborPicked[i] = 0;
    //初始化为less flat点
    cloudLabel[i] = 0;

    //每个scan，只有第一个符合的点会进来，因为每个scan的点都在一起存放
    if (int(laserCloud->points[i].intensity) != scanCount) {
      scanCount = int(laserCloud->points[i].intensity);//控制每个scan只进入第一个点

      //曲率只取同一个scan计算出来的，跨scan计算的曲率非法，排除，也即排除每个scan的前后五个点
      if (scanCount > 0 && scanCount < N_SCANS) {
        scanStartInd[scanCount] = i + 5;// scan起始位置的索引（去除前5个点）
        scanEndInd[scanCount - 1] = i - 5;// scan终止位置的索引（去除后5个点）
      }
    }
  }
  //第一个scan曲率点有效点序从第5个开始，最后一个激光线结束点序size-5
  scanStartInd[0] = 5;
  scanEndInd.back() = cloudSize - 5;

接下来排除瑕点： 1、避免周围点已被选择从而保证特征点分布均匀，或者局部平行于激光束的局部平面上的点。如图（a）中的点B 2、避免在被遮挡的边界上的点。如图（b）中的点A右侧被B遮挡住了

代码分为两部分：先从第二种排除容易被斜面挡住的点以及离群点开始。具体方式为先计算相邻两点的深度差，当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），然后再把深度大的点的一侧五个点设为处理过的点

上图，先计算前后两点AB的曲率，如果曲率diff>0.1,表明两点有距离差，然后分别计算AB点的深度，根据深度比例，统一到同一水平上，A根据比例变成A'，拿A'和B作比较，先计算二者的距离差，再和B做边长比（弧度），弧度小于0.1表明夹角小，斜面陡峭，把A和A之前的5个点都删除。

// 2.挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，
  // 而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
  for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值，所以减6
    float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
    float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
    float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
    //计算有效曲率点与后一个点之间的距离平方和
    float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;

    if (diff > 0.1) {//前提:两个点之间距离要大于0.1

        //点的深度（x^2+y^2+z^2）
      float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 
                     laserCloud->points[i].y * laserCloud->points[i].y +
                     laserCloud->points[i].z * laserCloud->points[i].z);

      //后一个点的深度
      float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 
                     laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
                     laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);

      //按照两点的深度的比例，将深度较大的点拉回后（按比例缩放）计算距离
      if (depth1 > depth2) {
        // X(i+1)-X(i)*（|X(i+1)|/|X(i)|）
        diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
        diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
        diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;

        // 边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上
        // 1.排除容易被斜面挡住的点，第二种情况
        if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
            // 深度大的点，该点及前面五个点（大致都在斜面上）全部置为筛选过
          cloudNeighborPicked[i - 5] = 1;
          cloudNeighborPicked[i - 4] = 1;
          cloudNeighborPicked[i - 3] = 1;
          cloudNeighborPicked[i - 2] = 1;
          cloudNeighborPicked[i - 1] = 1;
          cloudNeighborPicked[i] = 1;
        }
      } // 深度大的点，该点及后面的5个点全部设置为筛选过 
      else {
        diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
        diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
        diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;

        if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
          cloudNeighborPicked[i + 1] = 1;
          cloudNeighborPicked[i + 2] = 1;
          cloudNeighborPicked[i + 3] = 1;
          cloudNeighborPicked[i + 4] = 1;
          cloudNeighborPicked[i + 5] = 1;
          cloudNeighborPicked[i + 6] = 1;
        }
      }
    }

再处理第一种点，和激光平行的点。前后点平方和大于深度平方和的0.0002，这些点视为离群点。

// 2. 剔除第1种点，点与激光线平行
    float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
    float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
    float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
    //与前一个点的距离平方和
    float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;

    //点深度的平方和
    float dis = laserCloud->points[i].x * laserCloud->points[i].x
              + laserCloud->points[i].y * laserCloud->points[i].y
              + laserCloud->points[i].z * laserCloud->points[i].z;

    // 与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
    if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
      cloudNeighborPicked[i] = 1;
    }
  }

5、根据曲率排序，提取边缘点、平面点

首先对于每一层激光点(总16层)，将每层区域分成6份，起始位置为sp，终止位置为ep，然后对曲率从小到大进行排序。

for (int i = 0; i < N_SCANS; i++) {
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);

    //将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点，起始sp,终点ep
    for (int j = 0; j < 6; j++) {
        //六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
      int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;
      //六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
      int ep = (scanStartInd[i] * (5 - j)  + scanEndInd[i] * (j + 1)) / 6 - 1;

      //按曲率从小到大冒泡排序
      for (int k = sp + 1; k <= ep; k++) {
        for (int l = k; l >= sp + 1; l--) {
            //如果后面曲率点大于前面，则交换
          if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
            int temp = cloudSortInd[l - 1];
            cloudSortInd[l - 1] = cloudSortInd[l];
            cloudSortInd[l] = temp;
          }
        }
      }

接下来挑选边缘点，挑选每个分段的曲率很大和比较大的点。

//2.边缘点：挑选每个分段的曲率很大和比较大的点
      int largestPickedNum = 0;
      for (int k = ep; k >= sp; k--) {
        int ind = cloudSortInd[k];  //曲率最大点的点序

        //如果曲率大的点，曲率的确比较大，并且未被筛选过滤掉
        if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1) {

          largestPickedNum++;
          //挑选曲率最大的前2个点放入sharp点集合
          if (largestPickedNum <= 2) {
            cloudLabel[ind] = 2;//2代表点曲率很大
            cornerPointsSharp.push_back(laserCloud->points[ind]);
            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
          } 
          //挑选曲率最大的前20个点放入less sharp点集合
          else if (largestPickedNum <= 20) {
            cloudLabel[ind] = 1;//1代表点曲率比较尖锐
            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
          } 
          else {
            break;
          }

          cloudNeighborPicked[ind] = 1;//筛选标志置位

          //将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀
          for (int l = 1; l <= 5; l++) {
            float diffX = laserCloud->points[ind + l].x 
                        - laserCloud->points[ind + l - 1].x;
            float diffY = laserCloud->points[ind + l].y 
                        - laserCloud->points[ind + l - 1].y;
            float diffZ = laserCloud->points[ind + l].z 
                        - laserCloud->points[ind + l - 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {/
              break;
            }
            // 距离较近的点筛选出去。
            cloudNeighborPicked[ind + l] = 1;
          }
          for (int l = -1; l >= -5; l--) {
            float diffX = laserCloud->points[ind + l].x 
                        - laserCloud->points[ind + l + 1].x;
            float diffY = laserCloud->points[ind + l].y 
                        - laserCloud->points[ind + l + 1].y;
            float diffZ = laserCloud->points[ind + l].z 
                        - laserCloud->points[ind + l + 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
              break;
            }
            cloudNeighborPicked[ind + l] = 1;
          }
        }
      }

再找平面点

//3.平面点：挑选每个分段的曲率很小比较小的点
      int smallestPickedNum = 0;
      for (int k = sp; k <= ep; k++) {
        int ind = cloudSortInd[k];

        //如果曲率的确比较小，并且未被筛选出
        if (cloudNeighborPicked[ind] == 0 &&
            cloudCurvature[ind] < 0.1) {

          cloudLabel[ind] = -1;//-1代表曲率很小的点
          surfPointsFlat.push_back(laserCloud->points[ind]);

          smallestPickedNum++;
          if (smallestPickedNum >= 4) {//只选最小的四个，剩下的Label==0,就都是曲率比较小的
            break;
          }

          cloudNeighborPicked[ind] = 1;
          for (int l = 1; l <= 5; l++) {//同样防止特征点聚集
            float diffX = laserCloud->points[ind + l].x 
                        - laserCloud->points[ind + l - 1].x;
            float diffY = laserCloud->points[ind + l].y 
                        - laserCloud->points[ind + l - 1].y;
            float diffZ = laserCloud->points[ind + l].z 
                        - laserCloud->points[ind + l - 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
              break;
            }

            cloudNeighborPicked[ind + l] = 1;
          }
          for (int l = -1; l >= -5; l--) {
            float diffX = laserCloud->points[ind + l].x 
                        - laserCloud->points[ind + l + 1].x;
            float diffY = laserCloud->points[ind + l].y 
                        - laserCloud->points[ind + l + 1].y;
            float diffZ = laserCloud->points[ind + l].z 
                        - laserCloud->points[ind + l + 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
              break;
            }

            cloudNeighborPicked[ind + l] = 1;
          }
        }
      }

最后把剩余点（包括之前被排除的点）全部归入平面点中less flat类别中

// 4.将剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中
      for (int k = sp; k <= ep; k++) {
        if (cloudLabel[k] <= 0) {
          surfPointsLessFlatScan->push_back(laserCloud->points[k]);
        }
      }
    }

    //由于less flat点最多，对每个分段less flat的点进行体素栅格滤波
    pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.filter(surfPointsLessFlatScanDS);

    //less flat点汇总
    surfPointsLessFlat += surfPointsLessFlatScanDS;

6、将不同的特征点打包成消息发送出去

//publich消除非匀速运动畸变后的所有的点
  sensor_msgs::PointCloud2 laserCloudOutMsg;
  pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
  laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
  laserCloudOutMsg.header.frame_id = "/camera";
  pubLaserCloud.publish(laserCloudOutMsg);

  //publich消除非匀速运动畸变后的平面点和边沿点
  sensor_msgs::PointCloud2 cornerPointsSharpMsg;
  pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
  cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
  cornerPointsSharpMsg.header.frame_id = "/camera";
  pubCornerPointsSharp.publish(cornerPointsSharpMsg);

  sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
  pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
  cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
  cornerPointsLessSharpMsg.header.frame_id = "/camera";
  pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

  sensor_msgs::PointCloud2 surfPointsFlat2;
  pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
  surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
  surfPointsFlat2.header.frame_id = "/camera";
  pubSurfPointsFlat.publish(surfPointsFlat2);

  sensor_msgs::PointCloud2 surfPointsLessFlat2;
  pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
  surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
  surfPointsLessFlat2.header.frame_id = "/camera";
  pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

  //publich IMU消息,由于循环到了最后，因此是Cur都是代表最后一个点，即最后一个点的欧拉角，畸变位移及一个点云周期增加的速度
  pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);
  //起始点欧拉角
  imuTrans.points[0].x = imuPitchStart;
  imuTrans.points[0].y = imuYawStart;
  imuTrans.points[0].z = imuRollStart;

  //最后一个点的欧拉角
  imuTrans.points[1].x = imuPitchCur;
  imuTrans.points[1].y = imuYawCur;
  imuTrans.points[1].z = imuRollCur;

  //最后一个点相对于第一个点的畸变位移和速度
  imuTrans.points[2].x = imuShiftFromStartXCur;
  imuTrans.points[2].y = imuShiftFromStartYCur;
  imuTrans.points[2].z = imuShiftFromStartZCur;

  imuTrans.points[3].x = imuVeloFromStartXCur;
  imuTrans.points[3].y = imuVeloFromStartYCur;
  imuTrans.points[3].z = imuVeloFromStartZCur;

  sensor_msgs::PointCloud2 imuTransMsg;
  pcl::toROSMsg(imuTrans, imuTransMsg);
  imuTransMsg.header.stamp = laserCloudMsg->header.stamp;
  imuTransMsg.header.frame_id = "/camera";
  pubImuTrans.publish(imuTransMsg);

回调函数 imuHandler()

减去重力对imu的影响，求出xyz轴的加速度实际值，将加速度转换到世界坐标系。

//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  // 获得roll,pitch,yaw
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  // 减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系,
  // 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
  float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
  float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
  float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

  //循环移位效果，形成环形数组
  imuPointerLast = (imuPointerLast + 1) % imuQueLength;

  imuTime[imuPointerLast] = imuIn->header.stamp.toSec();

  imuRoll[imuPointerLast] = roll;
  imuPitch[imuPointerLast] = pitch;
  imuYaw[imuPointerLast] = yaw;
  imuAccX[imuPointerLast] = accX;
  imuAccY[imuPointerLast] = accY;
  imuAccZ[imuPointerLast] = accZ;

  AccumulateIMUShift();
}

AccumulateIMUShift()

进行航距推算，假定为匀速运动推算出当前时刻的位置、速度。

//积分速度与位移
void AccumulateIMUShift()
{
  float roll = imuRoll[imuPointerLast];
  float pitch = imuPitch[imuPointerLast];
  float yaw = imuYaw[imuPointerLast];
  float accX = imuAccX[imuPointerLast];
  float accY = imuAccY[imuPointerLast];
  float accZ = imuAccZ[imuPointerLast];

  // 绕RPY旋转转换得到世界坐标系下的加速度值
  //绕z轴旋转(roll)
  float x1 = cos(roll) * accX - sin(roll) * accY;
  float y1 = sin(roll) * accX + cos(roll) * accY;
  float z1 = accZ;
  //绕x轴旋转(pitch)
  float x2 = x1;
  float y2 = cos(pitch) * y1 - sin(pitch) * z1;
  float z2 = sin(pitch) * y1 + cos(pitch) * z1;
  //绕y轴旋转(yaw)
  accX = cos(yaw) * x2 + sin(yaw) * z2;
  accY = y2;
  accZ = -sin(yaw) * x2 + cos(yaw) * z2;

  //上一个imu点
  int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
  //上一个点到当前点所经历的时间，即计算imu测量周期
  double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];


  //求每个imu时间点的位移Shift与速度Velo,两点之间视为匀加速直线运动
  //要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义
  if (timeDiff < scanPeriod) {//（隐含从静止开始运动）

    imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 
                              + accX * timeDiff * timeDiff / 2;
    imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff 
                              + accY * timeDiff * timeDiff / 2;
    imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff 
                              + accZ * timeDiff * timeDiff / 2;

    imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
    imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
    imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
  }
}

参考： https://zhuanlan.zhihu.com/p/29719106 https://blog.csdn.net/HelloJinYe/article/details/100619907 https://blog.csdn.net/qq_21842097/article/details/80714483

二狗学长好

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
修改iview源码无效_LOAM源码解析1——scanRegistration

入手激光雷达SLAM，从最经典的LOAM源码开始，之后会对秦通在此基础上改进的A-LOAM源码进行观看，应该就会很简单了，第三步是把LEGO-LOAM的源码进行解读。之前对LOAM的论文进行了详细解读。LOAM的源码主要分为scanRegistration提取特征点、laserOdometry 10HZ估计位姿、laserMapping 1HZ构建三维地图、transforMaintenance...
复制链接

扫一扫