Codes that find 3 nearest points to fit a plane

[whyend1119]

In PointOdometry.cc, line #450, function void PointOdometry::Process():

`

         float point_sq_dis, point_sq_dis2 = 25, point_sq_dis3 = 25;
          for (int j = closest_point_idx + 1; j < last_surf_size; j++) {
            if (int(last_surf_cloud_->points[j].intensity) > closestPointScan + 2.5) {
              break;
            }

            point_sq_dis = CalcSquaredDiff(last_surf_cloud_->points[j], point_sel);

            if (int(last_surf_cloud_->points[j].intensity) <= closestPointScan) {
              if (point_sq_dis < point_sq_dis2) {
                point_sq_dis2 = point_sq_dis;
                second_closet_point_idx = j;
              }
            } else {
              if (point_sq_dis < point_sq_dis3) {
                point_sq_dis3 = point_sq_dis;
                third_clost_point_idx = j;
              }
            }
          }
          for (int j = closest_point_idx - 1; j >= 0; j--) {
            if (int(last_surf_cloud_->points[j].intensity) < closestPointScan - 2.5) {
              break;
            }

            point_sq_dis = CalcSquaredDiff(last_surf_cloud_->points[j], point_sel);

            if (int(last_surf_cloud_->points[j].intensity) >= closestPointScan) {
              if (point_sq_dis < point_sq_dis2) {
                point_sq_dis2 = point_sq_dis;
                second_closet_point_idx = j;
              }
            } else {
              if (point_sq_dis < point_sq_dis3) {
                point_sq_dis3 = point_sq_dis;
                third_clost_point_idx = j;
              }
            }
          }

` It seems that these codes are trying to find the 2nd and 3rd nearest point in those points next to(index increasing) and then previous to(index decreasing) the 1st nearest point.

However when loop in the increasing order, the code seems to find the 2nd nearest only in the lower rings(int(last_surfcloud->points[j].intensity) <= closestPointScan), and 3rd nearest only in the upper rings. Similar happen in the decreasing order.

I would like to know if it is a typo? or some consideration behind this? for by intuition, we want to find the 2nd & 3rd in both direction at the same time. Thanks for any help.

[hyye]

For odometry, i.e., scan to scan match, we follow LOAM's point distance. If I recall correctly, it will find (j, l, m) as the corresponding planar patch in LOAM. https://github.com/hyye/lio-mapping/blob/8485a4d9c359b7e486ad019a10815ecb8fd1257f/src/point_processor/PointOdometry.cc#L491-L493