Implement LIO-SAM algorithm

wheitman commented 2 years ago

Problem

Our existing mapping algorithm, which is based on NDT with no optimizations whatsoever, produces poor maps in the real world. The current implementation lacks:

LiDAR deskew, which removes the drift imposed by the rotation of the LiDAR sensors as the car moves
NDT initial guesses using IMU data
GPS ground truth "snapping"
Loop closure, manual or automatic

Possible solutions

Adapt LIO-SAM (paper) to run on our stack. This will require updating our Velodyne ROS wrapper along with other changes that have yet to be discovered.

Success condition

A single PCD file (let's say grand_loop.pcd for dramatic purposes) is formed.

grand_loop.pcd covers the entirety of the 2.01 mile/ 3.23 km loop.

grand_loop.pcd is free from noticeable noise or skew, such as misalignment of Lidar sensors, curvature from speed bumps, and so on.

The beginning of grand_loop.pcd aligns perfectly with the end, a.k.a. loop closure is successfully applied.

The map aligns well with the satelite ground truth.

wheitman commented 2 years ago

Paper is read. Source code is about halfway read and commented. Trying to modify so that Lidar ring data is optional. This would allow us to run on SVL. After code is read, will try to test in SVL, then on Hail Bopp.

wheitman commented 2 years ago

Regarding ring data, you can emulate (hack, really) ring data using something like this (found in a script somewhere):

        depth = np.linalg.norm(scan, 2, axis=1)
        pitch = np.arcsin(scan[:, 2] / depth) # arcsin(z, depth)
        fov_down = -24.8 / 180.0 * np.pi
        fov = (abs(-24.8) + abs(2.0)) / 180.0 * np.pi
        proj_y = (pitch + abs(fov_down)) / fov  # in [0.0, 1.0]
        proj_y *= 64  # in [0.0, H]
        proj_y = np.floor(proj_y)
        proj_y = np.minimum(64 - 1, proj_y)
        proj_y = np.maximum(0, proj_y).astype(np.int32)  # in [0,H-1]
        proj_y = proj_y.reshape(-1, 1)
        scan = np.concatenate((scan,proj_y), axis=1)
        scan = scan.tolist()
        for i in range(len(scan)):
            scan[i][-1] = int(scan[i][-1])

Basically apply trig to find the height of a point, then map this to an integer from 0-15 (for our 16-channel VLP-16 lidar).

This emulation probably nullifies any efficiency gain of using ring channels in the first place, but we'll only need to use this hack when running on the Quad, so who cares?

We should eventually rewrite this to remove the need for ring data entirely, but I'm in a crunch.

Nova-UTD / navigator