LOAM_velodyne学习笔记

参考https://zhuanlan.zhihu.com/p/29719106?utm_source=qq&utm_medium=social&utm_oi=1039233414704160768

针对b情况，A点本是一个面点，在这个时候会被误认为成一个角点。对应代码中的内容为：

遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。
(int i = 5; i < cloudSize - 6; i++) {
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; // points[i+1]与points[i]间的距离的平方

if (diff > 0.1) {      //判断A和B两点间距离的平方是否大于阈值0.1

  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); // points[i]的深度

  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); // points[i+1]的深度

  /* 针对论文中(b)情况 */
  /*depth1>depth2 X[i+1]距离更近，远侧点标记不特征；depth1<depth2 X[i]距离更近，远侧点标记不特征*/
  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; // 构建了一个等腰三角形的底向量

    if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) { 
  // 根据等腰三角形性质，这一判断threshold=0.1实际表示X[i]向量与X[i+1]的夹角小于5.732度
      // cloudNeighborPicked 是考虑一个特征点周围不能再设置成特征约束的判断标志位
      cloudNeighborPicked[i - 5] = 1;
      cloudNeighborPicked[i - 4] = 1;
      cloudNeighborPicked[i - 3] = 1;
      cloudNeighborPicked[i - 2] = 1;
      cloudNeighborPicked[i - 1] = 1;
      cloudNeighborPicked[i] = 1;
    }
  } 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;
    }
  }
}
画图解释