Vehicle Odometry Correction Using Lidar Contour Points (Localization)

Question

I am currently working on a project where...

1. Vehicle travels from point A to B
2. GPS trajectory is used as a reference
3. Vehicle odometry (bicycle model without slip angle beta) was used to estimate a trajectory, which resulted in odometry error
4. I am now given sets of lidar contour points & lane data (timestamps aligned with those of gps & odometry data), which I would have to use to correct the error and prove it

Note: The vehicle trajectory data I am using is NOT closed-loop.

My current thought is to use these lidar contour points to obtain rotational/translational vector between each frame and reverse-update the values to the vehicle to update the trajectory. When lacking lidar data, I'd like to use camera lane data to supplement the information.

I have read many articles on ICP and pose SLAM, yet there are some characteristics about my data that seem to hinder my progress.

1. ICP requires 'reference' lidar/map points which I do not have. Can I use points at t = x as a reference to update the points at t = x + 1?
2. I do not get same amount of lidar contour points per frame. That is, my data isn't in a PCL format. For example, some frames have 30 contour points whereas some frames have 150. (Some frames lack contour points). It seems that ICP algorithms require size of vectors to be the same. What would be the clever way to approach the problem when the number of contour points is fluctuating?
3. The lane data are given in coefficients constituting 3rd degree polynomial with start/end range for your information.
4. I have currently ONLY applied ICP to calculate R&T when the number of contour points at time t & t+1 is equal, and came up with R matrix of size 360x360. How would I apply this to vehicle that is located at a single point?

I do not necessarily have to use ICP to solve the problem, yet being new to the topic, I am kind of stuck at the moment. I'd appreciate some opinion/feedback. Thank you!

Answer 1

ICP requires 'reference' lidar/map points which I do not have. Can I use points at t = x as a reference to update the points at t = x + 1?

Reference just refers to a reference frame. Which can be just the previous frame, or one obtained earlier. So yes you can match t=x and t=x+1. That is how LIDAR odometry works.

I do not get same amount of lidar contour points per frame. That is, my data isn't in a PCL format. For example, some frames have 30 contour points whereas some frames have 150. (Some frames lack contour points). It seems that ICP algorithms require size of vectors to be the same. What would be the clever way to approach the problem when the number of contour points is fluctuating?

This is not true. The whole point of ICP (Iterative closest Point) is that it doesn't require explicit matches. Two points are considered a match if they are the closest pair to each other.

I have currently ONLY applied ICP to calculate R&T when the number of contour points at time t & t+1 is equal, and came up with R matrix of size 360x360. How would I apply this to vehicle that is located at a single point?

You are doing something wrong. ICP should give you 1 $Rt$ which is composed of a 3x3 rotation matrix and 3x1 translation matrix.

The lane data are given in coefficients constituting 3rd degree polynomial with start/end range for your information.

I would probably ignore these. Or just convert them to points using the start and end of the line. You could do something like ICL(Iterative Closest Line), but it could make the implementation a lot more complicated.