Range check for output index of nearestKSearch()

[miroslavradojevic]

I have tried running A-LOAM on Velodyne lidar .bag file recordings and encountered a bug when processing point-clouds.
In laserOdometry.cpp, at line 390:
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
pointSearchInd is later used to refer to values from laserCloudCornerLast->points[] list

...
closestPointInd = pointSearchInd[0];
...
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
...

It happens that the index obtained from nearestKSearch() is a number that exceeds the length of the laserCloudCornerLast->points list and causes this node to stop.

What I added was index check after line 395:

if (closestPointInd >= 0 && closestPointInd < (int)laserCloudCornerLast->points.size()){  
... lines 397 to 481...
}

and the remainder of the code for that block (line 394-482) should go inside if-block to make sure list is not invoked with invalid index.

[jypjypjypjyp]

Hello! I get very few points after adding the index check.