Suppose I have M lidar contour points from t = k and N lidar contour points from t = k+1.
(Some of you might question why I have differing number of contour points, and the reason is that these lidar contour points have been filtered from raw point cloud data to only detect road edge, and sometimes these edges aren't detected.)
My goal is to localize the vehicle position using these lidar contour points. Currently, I have generated two plots to make comparison to the result of lidar localization.
- Plot 1. GPS trajectory plot (this will be my reference)
- Plot 2. Vehicle odometry trajectory plot (this has drift compared to GPS trajectory)
I need to prove that my lidar localized trajectory somewhat improved the drift that existed from plot 2.
My current approach: I am using ICP to solve the problem. Since I do not have 'absolute reference' point cloud, I am basically using lidar points at t = k as a reference to lidar points at t = k+1. Therefore, the reference point cloud is constantly being updated as I go. Once I derive the rotation & translation vector between t = k and t = k+1, I'd like to use that to correct the vehicle trajectory from t = k to t = k+1.
Note: Exception case is when lidar contour data at t = k + a is empty, in which I simply use odometry (dead reckoning) to predict the trajectory.
My difficulty lies with the fact that I am lost on how to apply the rotation and translation vector to the previous vehicle pos_x, previous vehicle pos_y, and heading angle theta to make the correction to the trajectory. I suppose the heading angle value doesn't really matter in terms of updating the next position since I am working with 'point' rather than shape. The code below is snippet from my work, where closest_rot_angle, closest_translation_x, and closest_translation_y are obtained from ICP iterations until it converges.
I think the problem may be at the update step for the 'input_arr[0:2]' which contains x & y coordinate for the current position of the vehicle. Please provide me with some spark on a good way to apply ICP results for localization!
# transform 'points' (using the calculated rotation and translation) c, s = math.cos(closest_rot_angle), math.sin(closest_rot_angle) rot = np.array([[c, -s], [s, c]]) aligned_points = np.dot(points, rot.T) aligned_points[:, 0] += closest_translation_x aligned_points[:, 1] += closest_translation_y aligned_target = np.dot(input_arr[0:2], rot.T) aligned_target += closest_translation_x aligned_target += closest_translation_y # update 'points' for the next iteration points = aligned_points input_arr[0:2] = aligned_target