Support for unsynced+unrectified. Velodyne points with unique timestamps.

[fetty31]

Hello there,

I found my self with the need of using the KITTI dataset for testing a LiDAR based SLAM approach, so I decided to add support for unsynced+unrectified datasets.

I also took the liberty of adding a new argument related to velodyne points timestamps (-v, --velo). Due to the deskewing procedure done by most LiDAR based SLAMs, it's mandatory to have a timestamp for each point of the scan. Because this information is not given by the KITTI dataset, when --velo argument is given, each point's stamp is "approximated" based on the start and end sweep time. This way, the velodyne pointcloud will be published with the same fields as in the official ROS velodyne driver [x, y, z, intensity, time], where time is relative to the msg header stamp.

Finally, I adapted the use of progressbar library to python3, as it was the only module that failed when executing the script with python 3.8.10.

[ojura]

Hi @fetty31, thanks for your fork. Could you explain the difference between --v 1 and --v 2?

What does it mean "timestamp for each point (now [x, y, z, intensity, time]) in the same order as they're found in the dataset file" vs "timestamp for each point taking into account the LiDAR rotational motion"?

Btw, isn't the VLP-16 firing order of points in a packet known?

[fetty31]

Hi @ojura, --v 1 assumes the lidar points in the dataset file are ordered by their azimuth angle so it adds a unique timestamp for each point based on that assumption. Later I found out that the points are also ordered by channel, so the --v 1 isn't useful at all right now.

--v 2 adds a unique timestamp for each point based on the computed azimuth angle. Knowing the start and end time of each sweep, it's a linear interpolation.

[ojura]

Thanks for the response. Interpolating time by azimuth is super clear - consider adding that to the description for --v 2?

If the points are ordered by channel, maybe we could still reorder them into VLP 16's original firing order?

But that's probably too small of a diffence if by azimuth is probably good enough for unwarping.

[ojura]

@fetty31 thanks! I have spent a little more time playing with Kitti, but I ended up really disappointed.

If I am not mistaken, it looks like that even the “raw” (un)synced lidar data is not actually raw output from the sensor and has a baked in motion compensation. I examined these “raw” lidar point clouds and can’t find the rollover seam which should be visible in the scans, especially when the vehicle is making a turn.

My lidar slam algorithm is all based around solving for motion compensation and physically correct continous fusing of lidar data. It untwists the scans based on the estimated motion, so when I slam Kitti, it looks awful because it has apparently already been untwisted. (And it‘s a bit of a pain to disable it since the solver is all based around that.)

How did you end up testing this? Is there a proper raw, unmodified version of the Kitti lidar data available? Since all points in the scan have been untwisted, the time for all points should technically be zero, because they were reprojected as if observed from a single point in time. The azimuth approximation would be nice if the scans were unmodified, but it looks like that is not the case :/

[ojura]

@fetty31 sorry for spamming your Github, but thought you might want to know this. :)

Perhaps I was wrong in the post above, and the scans are not pre-unwarped.

It looks like the time rollover point is not in a constant direction, but varying per laser; and it looks like it is not in the forward direction, but generally in the direction of vehicle backward:

You have to wait for a bump in the road, and then the per-ring time rollover points reveal themselves:

[ojura]

@fetty31, FYI the Kitti motion compensation library also approximates using the back of the vehicle as the scan beginning/end:
https://github.com/fracgawd/kitti_motion_compensation/blob/e804645d1f598c8d1835c6f26b0a6292fe09440e/src/kitti_motion_compensation/timestamp_mocking.cpp#L46C3-L46C65

[fetty31]

Hi @ojura, thank you for this information! I couldn't look much into it as I'm also testing with other datasets. I appreciate a lot your comments, they have been really helpful. FYI I'm going to add the official timestamp interpolation to the pull request and remove the --v 1 option as it doesn't make any sense.

[ojura]

@fetty31 I have one more update for you. I measured the angle offsets of time rollovers for each ring on Sequence 2, and it looks like that they are constant (at least for this sequence).

Here's some C++ for calculating the ring and subtracting the measured offsets:

              const auto getQuadrant = [](const Eigen::Vector3f &point) {
                    int res = 0;
                    const float &x = point.x();
                    const float &y = point.y();
                    if (x > 0 && y >= 0)
                        res = 1;
                    else if (x <= 0 && y > 0)
                        res = 2;
                    else if (x < 0 && y <= 0)
                        res = 3;
                    else if (x >= 0 && y < 0)
                        res = 4;
                    return res;
                };

                int previous_quadrant = 0;
                int ring = 0;

                for (int i = cloud.size() - 1; i > -1; i--) {
                    auto &point = cloud[i];
                    int quadrant = getQuadrant(point.position);

                    const bool newRing = quadrant == 4 && previous_quadrant == 1 && ring < 64 - 1;
                    if (newRing) {
                        ring += 1;
                    }

                    constexpr float ringOffsets[] = {0.f /* 0 */,
                                                     0.f /* 1 */,
                                                     0.f /* 2 */,
                                                     0.f /* 3 */,
                                                     0.f /* 4 */,
                                                     0.f /* 5 */,
                                                     0.f /* 6 */,
                                                     0.f /* 7 */,
                                                     0.f /* 8 */,
                                                     0.f /* 9 */,
                                                     0.f /* 10 */,
                                                     0.f /* 11 */,
                                                     0.0380041 /* 12 */,
                                                     -0.0129953072 /* 13 */,
                                                     -0.0791403072 /* 14 */,
                                                     -0.132105307 /* 15 */,
                                                     0.133803 /* 16 */,
                                                     0.082148 /* 17 */,
                                                     0.01689695 /* 18 */,
                                                     -0.0350753072 /* 19 */,
                                                     -0.0995103072 /* 20 */,
                                                     -0.151635307 /* 21 */,
                                                     0.154363 /* 22 */,
                                                     0.1011292 /* 23 */,
                                                     0.03964 /* 24 */,
                                                     -0.0142503072 /* 25 */,
                                                     -0.0779853072 /* 26 */,
                                                     -0.129145307 /* 27 */,
                                                     0.132703 /* 28 */,
                                                     0.0792194 /* 29 */,
                                                     0.01748945 /* 30 */,
                                                     -0.0344203072 /* 31 */,
                                                     0.0263602 /* 32 */,
                                                     -0.00909530718 /* 33 */,
                                                     -0.0483653072 /* 34 */,
                                                     -0.0837953072 /* 35 */,
                                                     0.0875975 /* 36 */,
                                                     0.0541753 /* 37 */,
                                                     0.0132506 /* 38 */,
                                                     -0.0230503072 /* 39 */,
                                                     -0.0611153072 /* 40 */,
                                                     -0.0974053072 /* 41 */,
                                                     0.101675 /* 42 */,
                                                     0.06450815 /* 43 */,
                                                     0.0277802 /* 44 */,
                                                     -0.00998030718 /* 45 */,
                                                     -0.0462503072 /* 46 */,
                                                     -0.0840103072 /* 47 */,
                                                     0.0884098 /* 48 */,
                                                     0.05149045 /* 49 */,
                                                     0.01465825 /* 50 */,
                                                     -0.0236653072 /* 51 */,
                                                     -0.0595453072 /* 52 */,
                                                     -0.0971503072 /* 53 */,
                                                     0.1024695 /* 54 */,
                                                     0.06471675 /* 55 */,
                                                     0.02833535 /* 56 */,
                                                     -0.00994530718 /* 57 */,
                                                     -0.0465653072 /* 58 */,
                                                     -0.0858253072 /* 59 */,
                                                     0.0894418 /* 60 */,
                                                     0.05225435 /* 61 */,
                                                     0.0151701 /* 62 */,
                                                     -0.0229053072 /* 63 */};

                    float timeInRing = M_PI - std::atan2(point.position.y(), point.position.x()) - ringOffsets[ring];
                    while (timeInRing > 2 * M_PI)
                        timeInRing -= 2 * M_PI;
                    while (timeInRing < 0)
                        timeInRing += 2 * M_PI;

                    point.time = length * (timeInRing / (2.0 * M_PI);

Unfortunately, I couldn't measure it exactly for lasers 0-11 because there are no measurements near the back due to the car being close. The offsets do make a pattern that we could perhaps extrapolate for lasers 0-11.

[ojura]

@fetty31 just to illustrate what those offsets do, here is a sweep from a turn in sequence 02:

You can observe that the tree was observed twice, right around the rollover discontinuity. Some lasers observe it at the begin of the sweep, and others at the end.

Unfortunately, the point cloud of Kitti still looks horrible for me compared to how the algorithm performs on other datasets. I hope you have more luck :)