Applying Rotation & Translation Matrix Obtained from Iterative Closest Point

Question

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[0] += closest_translation_x
        aligned_target[1] += closest_translation_y

        # update 'points' for the next iteration
        points = aligned_points
        input_arr[0:2] = aligned_target

Answer 1

I don't know if your confusion is with applying the transform to the points or applying it to the pose. So I'll just show you both.

The easiest way is to store your points and transform in the homogenous form. For 2D the transform is a matrix(3x3) that looks like

$$T=\begin{bmatrix} cos(\theta) & -sin(\theta) & t_x\\ sin(\theta) & cos(\theta) & t_y \\ 0 & 0 & 1 \end{bmatrix} $$

Points:

A point in homogenous form just has a 1 appended at the end.

$$p=[x,y,1]$$

You can now multiply a point by this transform matrix to get the new position.

$$p_b=T_{ba}*p_a$$

where $a$ and $b$ are some coordinate frames. If you actually break this down you can see that the multiplication is the same as your code.

$$p_b=R_{ba}*p_a+t_{ba}$$

Poses:

Your vehicle pose of $[t_x,t_y,\theta]$ can be represented as a homogenous matrix. The nice thing about this form is that you can concatenate multiple transforms together with just matrix multiplication.

$$T_a=T_b*T_{ba}$$

In your application ICP solves for the delta transform between two coordinate frames. So at every timestep solve for the delta using ICP and multiply your previous pose.

$$T_{k+1}=T_{k}*T_{k,k+1}$$

Note depending on your ICP algorithm your answer may be the inverse transform so $T_{k+1,k}$. Another nice property of the homogenous matrix is that the matrix inverse also is also the inverse of the transform.

$$T_{k+1,k}^{-1}=T_{k,k+1}$$