Point interpolation error

[StefanGlaser]

Hi, I'm currently trying to improve the implementation of this algorithm. As part of my refactorings, I changed the calculation of the ring start and end indices. They are now calculated by accumulating the sizes of the individual ring clouds. After doing so, I noticed a slight difference in the results and took a closer look again.

I've added

ROS_INFO("Scan count changed to: %d", scanCount);

after line 384 in scanRegistration.cpp to output the scanCount variable each time it changes.

While for some scans, the output matches the expected sequence, namely:

[ INFO] [1511533007.272980959]: Scan count changed to: 0
[ INFO] [1511533007.273194307]: Scan count changed to: 1
[ INFO] [1511533007.273374062]: Scan count changed to: 2
[ INFO] [1511533007.273552746]: Scan count changed to: 3
[ INFO] [1511533007.273726436]: Scan count changed to: 4
[ INFO] [1511533007.273903323]: Scan count changed to: 5
[ INFO] [1511533007.274091778]: Scan count changed to: 6
[ INFO] [1511533007.274272015]: Scan count changed to: 7
[ INFO] [1511533007.274449377]: Scan count changed to: 8
[ INFO] [1511533007.274637479]: Scan count changed to: 9
[ INFO] [1511533007.274820746]: Scan count changed to: 10
[ INFO] [1511533007.275001383]: Scan count changed to: 11
[ INFO] [1511533007.275172558]: Scan count changed to: 12
[ INFO] [1511533007.275348314]: Scan count changed to: 13
[ INFO] [1511533007.275523124]: Scan count changed to: 14
[ INFO] [1511533007.275698830]: Scan count changed to: 15

some other scans show the following output:

[ INFO] [1511533006.468410449]: Scan count changed to: 0
[ INFO] [1511533006.468714465]: Scan count changed to: 1
[ INFO] [1511533006.468884878]: Scan count changed to: 0
[ INFO] [1511533006.468991116]: Scan count changed to: 1
[ INFO] [1511533006.469013792]: Scan count changed to: 2
[ INFO] [1511533006.469197859]: Scan count changed to: 1
[ INFO] [1511533006.469282555]: Scan count changed to: 2
[ INFO] [1511533006.469309297]: Scan count changed to: 3
[ INFO] [1511533006.469491094]: Scan count changed to: 2
[ INFO] [1511533006.469575016]: Scan count changed to: 3
[ INFO] [1511533006.469601585]: Scan count changed to: 4
[ INFO] [1511533006.469782444]: Scan count changed to: 3
[ INFO] [1511533006.469866047]: Scan count changed to: 4
[ INFO] [1511533006.469892284]: Scan count changed to: 5
[ INFO] [1511533006.470089060]: Scan count changed to: 4
[ INFO] [1511533006.470172099]: Scan count changed to: 5
[ INFO] [1511533006.470200482]: Scan count changed to: 6
[ INFO] [1511533006.470382239]: Scan count changed to: 5
[ INFO] [1511533006.470465808]: Scan count changed to: 6
[ INFO] [1511533006.470491802]: Scan count changed to: 7
[ INFO] [1511533006.470676708]: Scan count changed to: 6
[ INFO] [1511533006.470759771]: Scan count changed to: 7
[ INFO] [1511533006.470788122]: Scan count changed to: 8
[ INFO] [1511533006.470968738]: Scan count changed to: 7
[ INFO] [1511533006.471052407]: Scan count changed to: 8
[ INFO] [1511533006.471078689]: Scan count changed to: 9
[ INFO] [1511533006.471260493]: Scan count changed to: 8
[ INFO] [1511533006.471342678]: Scan count changed to: 9
[ INFO] [1511533006.471369029]: Scan count changed to: 10
[ INFO] [1511533006.471548978]: Scan count changed to: 9
[ INFO] [1511533006.471633309]: Scan count changed to: 10
[ INFO] [1511533006.471661731]: Scan count changed to: 11
[ INFO] [1511533006.471837152]: Scan count changed to: 10
[ INFO] [1511533006.471920503]: Scan count changed to: 11
[ INFO] [1511533006.471946920]: Scan count changed to: 12
[ INFO] [1511533006.472128592]: Scan count changed to: 11
[ INFO] [1511533006.472212757]: Scan count changed to: 12
[ INFO] [1511533006.472239363]: Scan count changed to: 13
[ INFO] [1511533006.472416895]: Scan count changed to: 12
[ INFO] [1511533006.472498789]: Scan count changed to: 13
[ INFO] [1511533006.472524533]: Scan count changed to: 14
[ INFO] [1511533006.472704906]: Scan count changed to: 13
[ INFO] [1511533006.472787776]: Scan count changed to: 14
[ INFO] [1511533006.472814264]: Scan count changed to: 15
[ INFO] [1511533006.472992890]: Scan count changed to: 14
[ INFO] [1511533006.473074925]: Scan count changed to: 15

As a result, the calculated scan start and end indices are sometimes not corresponding to the actual scan ring indices in the combined cloud. This even sometimes results in the scenario where the start index of the last ring is grater than its end index (since the end index is set explicitly afterwards).

The problem seems to originate in the calculation of the relative time of a point based on its angle, respectively the determination of the start and end angles. Tests have shown that (for a VLP-16) the scan range per received input cloud is slightly more than 360 degrees. This in combination with other issues in determining the start and end orientation probably produces negative relative time values, causing the point to "hop" to its predecessor ring.

This issue translates over to the lidarOdometry component, as it also uses the intensities of the points. However, I'm not sure yet about its implications there.

After fixing the scan start and end index calculation, the overall result is comparable, but slightly different. I can't tell if it's better or worse, yet.

[StefanGlaser]

By replacing the current point interpolation based on the horizontal angle of a point with a simple linear interpolation based on the index of a point within its ring, I get a noticeably improvement in the mapping performance on the test data set.

Example code (surely not compiling):

point.intensity = ringID + SCAN_PERIOD * pointInRingIdx / ringSize;

However, I'm not sure if points of rings always arrive in order and scans may also pose wholes due to several reasons. So the above approach is certainly not the final answer to the problem, but it shows that the current implementation has some flaws with negative effects on the resulting performance. I'll look into this in more detail and let you know once I've figured out a proper and efficient solution. Shouldn't be that tough... ;)

[facontidavide]

@StefanGlaser for your information, I am planning to send a PR to the velodyne driver itself to avoid the issue of the starting index :)

[jlblancoc]

Another FYI ;-) I plan to port to ROS (anytime this or next week?) our C++ implementation of Velodyne driver that, among other things, collect "rings" / "scans" in a consistent order, always pack them in 360deg segments, includes options to programmatically change the Dual/Single return mode, etc. etc.

I'll push changes to: https://github.com/mrpt-ros-pkg/mrpt_sensors I totally don't recommend using it for production, but testing will be more than welcome ;-)

[facontidavide]

Well it must be true that "great people think alike" ;)

That is very cool, thanks for mentioning it

[StefanGlaser]

To be honest, I didn't think about changing the driver. And I wonder if we would loose backward compatibility with certain datasets if we do so. I've already solved this issue with a refactoring of the corresponding logic. Just want to do some further testing before announcing my results.

On the other hand, getting reliable structured data would certainly simplify data extraction logic and probably also benefit the overall performance. Definitely worth a try.

[alittleharry]

@StefanGlaser in fact, i prefer to compute the ring and time in the driver, and make every scan a sorted pointcloud that the size for every ring is constant and same. similar with this https://github.com/01001HR/velodyne

[StefanGlaser]

After thinking about thins for some days I agree with you that changing the driver is certainly the best option. Getting reliable input data is crucial and ist's not the task of LOAM to handle all sort of driver issues.

[snehagn]

Hello all, This is really interesting an interesting issue. Since I am using the velodyne driver in my application, I was wondering if there was an update to it regarding the scan indices? Thank you!