Deskewing...again

[tizianoGuadagnino]

Motivation

Since the beginning of time we decided to deskew the input scans at the middle of the scan duration (0.5), the motivation for this was given in #299, and it is an heritage of our initial development based on KITTI (everybody has his dark little secrets). This PR aim at removing the magic parameter 0.5, and deskew the scans in a more principled way.

This PR

Our final solution is to first deskew the scans at the beginning of the scan duration. We made this choice as there is no easy way to know if the a scan has been stamped at the beginning or the end of the scan collection, especially from a python perspective. We go for the beginning as this is the most common, and also the "right" way of timestamping. We then transform the resulting point cloud in the local frame of the scan after applying the constant velocity initial guess, basically moving the scan to the end of the predicted motion. This new deskewed frame is than passed to the successive steps of the pipeline.

Results

On all tested sequences there is no significant difference between deskeing at 0.5 and the current version, so this can be seen as a style change and a way of having things in the right reference frame. This might be more relevant for low frequency scanners than for typical LiDARS running at 10Hz.

[benemer]

Some more insights on this:

We always thought it did not matter which timestamp of the scan duration we set the reference for deskewing; see #299. However, there is one thing we always missed: In our current implementation, it does mar at which timestamp we estimate the pose. If we set the deskewing reference timestamp to 0, we effectively estimate the pose at the beginning of the scan duration; with 1.0, we estimate the pose at the end of the scan duration, and with 0.5 (default KISS before this PR), we estimate the pose at the middle of the scan duration. Since the KITTI ground truth reports the pose at the middle of the scan duration, we chose 0.5 as explained in #299.

Why does it matter, and why do we want to change KISS to estimate the pose at the end of the scan? Because we use the same constant velocity estimate last_delta_ for the initial guess and for deskewing. Now, if you estimate the pose at the beginning of scan i, our last_delta_ describes the motion from the beginning of scan i-2 to the beginning of scan i-1, and you use it for deskewing from the beginning of scan i to the end of scan i. This means we assume a constant motion from the beginning of scan i-2 to the end of scan i, which is three scan durations (!).

Let's do the same by estimating the pose at the end of scan i (this PR). Our constant velocity estimate is the motion between the end of scan i-2 and the end of scan i-1; therefore, we assume a constant motion for only two scan durations (the end of scan i-2, which is the beginning of scan i-1, and the end of scan i).

In the case of non-linear motion (surprise, this is reality), this is a big deal. We observed that if we estimate the pose at the beginning (by setting mid_pose_timestamp = 0.0), the odometry is much worse when the vehicle accelerates. As explained above, our constant velocity estimate was "too old" for deskewing and a worse approximation than taking it from the estimated poses at the end of the scans.

Estimating the pose at the middle of the scan instead of at the end will yield better results. The constant velocity model is more accurate for deskewing.

TL;DR

When estimating the poses at the end of the scan duration, the constant velocity estimate we also use for deskewing is more accurate because it is closer in time to the actual motion during the current scan.