Point-Point ICP
If we write out our state vector as [theta, translation], the pseudo code for point-point ICP is:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
pt_pt_icp(pose_estimate, source_scan, target_scan):
build_kd_tree(target_scan)
for n in ITERATION_NUM:
for pt in source_scan:
pt_map = pose_estimate * pt
pt_map_match = kd_tree_nearest_neighbor_search(pt_map)
errors[pt] = pt_map_match - pt_map
jacobians[pt] = [pose_estimate.rotation() * hat(pt_map), -Identity(3)]
total_residual = errors* errors
H = sum(jacobians.transpose() * jacobians)
b = sum(-jacobians * errors)
dx = H.inverse() * b
pose_estimate += dx;
if get_total_error() -> constant:
return
Note that in Point-Point ICP, for scan matched point (in map frame) p_i and its matched point q_i, if pose estimate is decomposed into [R, t], we set:
Notes
- Iterative Method is not a must. Unlike the above point association -> pose estimate -> … iteration, recent scan matching methods solve point association and pose estimate together. TODO?
- Iterative methods are relatively simpler, though. It works as long as the cost landscape is decreasing from the current estimate to the global minimum.
Point-Plane ICP
If we write out our state vector as [theta, translation], the pseudo code for point-plane ICP is:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
pt_plane_icp(pose_estimate, source_scan, target_scan):
build_kd_tree(target_scan)
for n in ITERATION_NUM:
for pt in source_scan:
pt_map = pose_estimate * pt
N_pt_map_matches = kd_tree_nearest_neighbor_search(N, pt_map)
plane_coeffs = fit_plane(N_pt_map_matches)
errors[pt] = signed_distance_to_plane(plane_coeffs, pt_map)
jacobians[pt] = get_jacbian(plane_coeffs, pt_map)
total_residual = errors* errors
H = sum(jacobians.transpose() * jacobians)
b = sum(-jacobians * errors)
dx = H.inverse() * b
pose_estimate += dx;
if get_total_error() -> constant:
return
The Point-Plane ICP is different from Point-Point ICP in:
- We are trying to fit a plane instead of using point-point match
- In real life, we need a distance threshold for filtering out distance outliers.
Also note that:
- A plane is $n^T x + d = 0$. The plane coefficient we get are $n, d$
- The signed distance between a point and a plane is:
- The jacobian of error is:
Point-Line ICP
The point-line ICP is mostly similar to the point-plane ICP, except that we need to adjust the error and Jacobians.
A line is:
\[\begin{gather*} \begin{aligned} & d \vec{r} + p_0 \end{aligned} \end{gather*}\]We choose the error as the signed distance between a point and it is :
\[\begin{gather*} \begin{aligned} & e = \vec{r} \times (R p_t + t - p_0) \\ & = \vec{r} ^ \land (R p_t + t - p_0) \end{aligned} \end{gather*}\]The jacobian is:
\[\begin{gather*} \begin{aligned} & \frac{\partial e}{\partial R} = - \vec{r} ^\land R p_t^{\land} \\ & \frac{\partial e}{\partial t} = \vec{r} ^\land \end{aligned} \end{gather*}\]NDT (Normal Distribution Transform)
Comparison TODO
-
PCL is slower, why?
