Project Proposal: 2D Point Cloud Registration using the Iterative Closest Point (ICP) Algorithm

Course: VV570/MATH6003J: Introduction to Engineering Numerical Analysis 1

1. Introduction & Problem Statement

In robotic perception, sensors like LiDAR capture the environment as a “point cloud”—a set of data points in space. To enable tasks like mapping or localization, a robot must align multiple point clouds taken from different positions. This project will address this fundamental robotics problem by implementing the Iterative Closest Point (ICP) algorithm, a numerical optimization method used to find the optimal alignment between two point clouds2.

2. Objective

The primary objective of this project is to develop a working 2D implementation of the ICP algorithm. We will apply numerical analysis techniques to find the rigid transformation (rotation and translation) that minimizes the distance between two point clouds, demonstrating a practical application of concepts from the course to the field of robotics.

3. Methodology

Our approach will be to apply a computational tool to solve this problem3. We will use Python with the NumPy library for numerical computations and Matplotlib for visualization. The core steps of our implementation will be:

• Data Association: For each point in a source cloud, find the closest corresponding point in the target cloud.

• Numerical Optimization: Calculate the optimal rotation and translation that minimizes the mean squared error between the corresponding point pairs. This will be solved using Singular Value Decomposition (SVD).

• Iterative Refinement: Apply the calculated transformation to the source cloud and repeat the process until the alignment error converges below a set threshold.

We will start with the simplification of working in 2D and assume a reasonable initial alignment between the point clouds4.

4. Expected Outcomes & Justification of Results

By the end of this three-week project5, we will deliver:

• A Python implementation of the 2D ICP algorithm.

• Visualizations demonstrating the algorithm’s effectiveness by showing the point clouds before and after alignment.

• A plot of the alignment error over iterations, which will serve to justify our results by showing the convergence of the numerical method6.

Ex 1

a

The result of hw4_1_a.m:

Vandermonde matrices are known to be ill-conditioned, with the condition number growing exponentially with $n$.

d

This tridiagonal matrix is well-conditioned. Its condition number grows polynomially with $n$ (like $O(n^2)$). The error $\epsilon_n$ should grow much more slowly than in the Vandermonde case, exhibiting polynomial rather than exponential growth. The residual $r_n$ will be a better, though not perfect, indicator of the true error compared to the previous case.

Comments on the obtained results:

• $\textbf{Error Growth:}$ The relative error $\epsilon_n$ for the tridiagonal system grows polynomially with $n$, which is significantly slower than the exponential growth observed for the Vandermonde matrix. This will be visible as a straight line on the loglog plot.
• $\textbf{Conditioning:}$ The dramatic difference in error behavior is due to the condition number. The Vandermonde matrix is severely ill-conditioned, whereas the tridiagonal matrix is well-conditioned.
• $\textbf{Residual as an Error Indicator:}$ For the well-conditioned tridiagonal matrix, the normalized residual $r_n$ and the relative error $\epsilon_n$ have much closer values. The residual is a far more reliable indicator of the true error in this case than it was for the Vandermonde system.

Ex 2

a

The task is to complete the files $jacobi.m$ and $gauss_seidel.m$. These files implement iterative methods based on a matrix splitting $A = P - (P-A)$, where $P$ is the preconditioner. The iterative update can be expressed as $x^{(k+1)} = x^{(k)} + z^{(k)}$, where $z^{(k)}$ is the solution to $Pz^{(k)} = r^{(k)}$, and $r^{(k)} = b - Ax^{(k)}$ is the residual.

For the $\textbf{Jacobi method}$, the preconditioner P is the diagonal of A. For the $\textbf{Gauss-Seidel method}$, P is the lower triangular part of A. The missing lines implement the calculation of the residual, the update step, and the storing of the residual norm at each iteration.

jacobi.m:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

function [x, iter, res]= jacobi(A,b,x0,nmax,tol)
% JACOBI iterative method
%   [X, Niter, Res] = JACOBI(A,B,X0,NMAX,TOL) attempts to solve the system of linear equations A*X=B
%   for X using the Jacobi method.

% Jacobi preconditioner
P=diag(diag(A)); %

% residual normalization
r0=norm(b);
if r0==0, r0=1; end

% first iteration
x=x0;
r=b-A*x; % The residual b - A*x
res(1)=norm(r); % The norm of the initial residual
iter=0;

while res(end)/r0 > tol && iter < nmax
    z=P\r; % Solve Pz=r
    x=x+z; % Update the solution
    r=b-A*x; % Recompute the residual
    iter=iter+1;
    res(iter+1)=norm(r); % Store the norm of the new residual
end

return

gauss_seidel.m:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

function [x, iter, res]= gauss_seidel(A,b,x0,nmax,tol)
% GAUSS-SEIDEL iterative method
%   [X, Niter, Res] = GAUSS_SEIDEL(A,B,X0,NMAX,TOL) attempts to solve the system of linear equations A*X=B
%   for X using the Gauss-Seidel method.

% Gauss-Seidel preconditioner
P=tril(A); %

% residual normalization
r0=norm(b);
if r0==0, r0=1; end

% first iteration
x=x0;
r=b-A*x; % The residual b - A*x
res(1)=norm(r); % The norm of the initial residual
iter=0;

while res(end)/r0 > tol && iter < nmax
    z=P\r; % Solve Pz=r using forward substitution
    x=x+z; % Update the solution
    r=b-A*x; % Recompute the residual
    iter=iter+1;
    res(iter+1)=norm(r); % Store the norm of the new residual
end

return

b

I solve the system for $n=4$ with the tridiagonal matrix $A$ having $2.5$ on the diagonal and $-1$ on the off-diagonals. The right-hand side is $b=(1.5,0.5,0.5,1.5)^{\top}$. The initial guess is $x^{(0)}=0$, tolerance is $10^{-10}$, and max iterations is $10^8$. I compare the number of iterations and computing time.

The matrix $A$ is strictly diagonally dominant since $|a_{ii}| = 2.5 > \sum_{j \neq i} |a_{ij}|$ for all rows. This property guarantees that both methods will converge.

For this well-conditioned matrix (as shown in part e), the number of iterations grows very slowly with $n$. The condition number remains small and constant. The relative error and the normalized residual are very close in value, indicating that the residual is a good estimator of the error.

d

The analysis from (c) is repeated for the matrix $A$ with 2 on the diagonal and -1 on the off-diagonals , and $b=(1,0,…,0,1)^{\top}$. The exact solution is again $x=(1,…,1)^{\top}$. This matrix is only weakly diagonally dominant, so I expect slower convergence.

The number of iterations will grow much more rapidly with $n$. This is because, as shown in part (e), the condition number of this matrix grows quadratically with $n$, making the problem progressively harder to solve.

e

I use the given formula for the eigenvalues of a symmetric tridiagonal matrix $M$ with $a$ on the diagonal and $b$ on the off-diagonals:
$$ \lambda_{i}(M)=a+2b\cos\left(\frac{\pi i}{n+1}\right), \quad i=1,…,n $$
The condition number is $\kappa_2(A) = |\lambda_{max}|/|\lambda_{min}|$.

Case 1: Matrix from part (c)
Here, $a=2.5$ and $b=-1$. The eigenvalues are $\lambda_i = 2.5 - 2\cos(\frac{\pi i}{n+1})$.

• As $n\to\infty$, $\lambda_{max} \to 2.5 - 2(-1) = 4.5$.
• As $n\to\infty$, $\lambda_{min} \to 2.5 - 2(1) = 0.5$.

The condition number $\kappa(A)$ approaches a constant, $\kappa(A) \to 4.5 / 0.5 = 9$. This is consistent with the numerical results from part (c), explaining the fast and stable convergence.

Case 2: Matrix from part (d)
Here, $a=2$ and $b=-1$. The eigenvalues are $\lambda_i = 2 - 2\cos(\frac{\pi i}{n+1}) = 4\sin^2(\frac{\pi i}{2(n+1)})$.

• As $n\to\infty$, $\lambda_{max} \to 4\sin^2(\pi/2) = 4$.
• For large $n$, $\lambda_{min} = 4\sin^2(\frac{\pi}{2(n+1)}) \approx 4(\frac{\pi}{2(n+1)})^2 = \frac{\pi^2}{(n+1)^2}$.
  The condition number $\kappa(A) \approx \frac{4}{\pi^2/(n+1)^2} = O(n^2)$. This quadratic growth in the condition number is consistent with the numerical results from part (d), explaining the significant increase in iterations with $n$.

Exercise 1

Problem Analysis

The objective is to find the coefficients $(\alpha, \beta, \gamma)$ for the finite difference formula:
$$
D_{h}f(\overline{x})=\frac{\alpha f(\overline{x})+\beta f(\overline{x}-h)+\gamma f(\overline{x}-2h)}{h} \quad
$$
The analysis begins by substituting the Taylor series expansions for $f(\overline{x}-h)$ and $f(\overline{x}-2h)$ around the point $\overline{x}$ into the formula.

The expansions are:
$$
f(\overline{x}-h) = f(\overline{x}) - hf’(\overline{x}) + \frac{h^2}{2}f’’(\overline{x}) - \frac{h^3}{6}f’’’(\overline{x}) + O(h^4)
$$
$$
f(\overline{x}-2h) = f(\overline{x}) - 2hf’(\overline{x}) + 2h^2f’’(\overline{x}) - \frac{4h^3}{3}f’’’(\overline{x}) + O(h^4)
$$
Substituting these into the formula and grouping terms by derivatives of $f(\overline{x})$ results in:
$$
D_{h}f(\overline{x}) = \frac{\alpha + \beta + \gamma}{h} f(\overline{x}) + (-\beta - 2\gamma) f’(\overline{x}) + \left( \frac{\beta}{2} + 2\gamma \right)h f’’(\overline{x}) + O(h^2)
$$
To approximate $f’(\overline{x})$, the coefficient of $f’(\overline{x})$ must be $1$, and the coefficient of $f(\overline{x})$ must be $0$. This yields two necessary equations:

1. $\alpha + \beta + \gamma = 0$
2. $-\beta - 2\gamma = 1$

a) Solution for First-Order Accuracy

For the formula to be of order 1, the two fundamental equations must be satisfied.

From equation (2), $\beta$ can be expressed in terms of $\gamma$:
$$
\beta = -1 - 2\gamma
$$
Substituting this into equation (1):
$$
\alpha + (-1 - 2\gamma) + \gamma = 0 \implies \alpha = 1 + \gamma
$$
Therefore, the family of coefficients $(\alpha, \beta, \gamma)$ that provides at least first-order accuracy is given by:
$$
(\alpha, \beta, \gamma) = (1+\gamma, -1-2\gamma, \gamma), \quad \forall \gamma \in \mathbb{R}
$$

b) Solution for Second-Order Accuracy

To achieve second-order accuracy, the truncation error must be $O(h^2)$. This requires the coefficient of the $h$ term in the error expansion to also be zero.

$$\frac{\beta}{2} + 2\gamma = 0$$

This creates a system of three linear equations to be solved for the unique values of $(\alpha, \beta, \gamma)$:

1. $\alpha + \beta + \gamma = 0$
2. $-\beta - 2\gamma = 1$
3. $\frac{\beta}{2} + 2\gamma = 0$

From equation (3), we find that $\beta = -4\gamma$. Substituting this into equation (2):
$$
-(-4\gamma) - 2\gamma = 1 \implies 2\gamma = 1 \implies \gamma = \frac{1}{2}
$$
With the value for $\gamma$, $\beta$ can be found:
$$
\beta = -4 \left( \frac{1}{2} \right) = -2
$$
Finally, substituting $\beta$ and $\gamma$ into equation (1) yields $\alpha$:
$$
\alpha + (-2) + \frac{1}{2} = 0 \implies \alpha = \frac{3}{2}
$$
The unique values which give a formula of order 2 are:
$$
\left(\alpha, \beta, \gamma\right) = \left(\frac{3}{2}, -2, \frac{1}{2}\right)
$$

Exercise 2

a) Midpoint Formula Calculation

The first task is to compute the integral of the function $f(x)=e^{-x}sin(x)$ on the interval $[a,b]=[0,2]$. The computation uses the composite midpoint formula with $M=10$ sub-intervals.

The resulting numerical approximation of the integral is 0.468592. The exact integral is 0.466630. So the error is 0.001962.

b) Convergence of the Midpoint Formula

This section analyzes the convergence of the composite midpoint formula for the same function and interval. The number of sub-intervals is varied as $M=10^{1},10^{2},10^{3},…10^{5}$.

The absolute error, $|I(f)-Q_{h}^{pm}(f)|$, is computed for each corresponding step size, $h=(b-a)/M$. This error is the difference between the numerical result and the exact value of the integral, $I=(1-e^{-2}(sin(2)+cos(2)))/2$.

A graph of the error versus the step size $h$ is then created using a logarithmic scale for both axes. In such a plot, the slope of the resulting line corresponds to the order of convergence.

The plot displays a straight line with a negative slope. A linear fit to the logarithmic data reveals a slope of approximately 2. This visually confirms that the method exhibits 2nd-order convergence, which aligns with the theoretical error bounds for the composite midpoint rule.

c) Convergence of Trapezoidal and Simpson’s Formulas

The analysis is repeated using the composite trapezoidal formula and the composite Simpson’s formula. The errors for both methods, $|I(f)-Q_{h}^{trap}(f)|$ and $|I(f)-Q_{h}^{simp}(f)|$, are plotted on the same graph in logarithmic scale.

The resulting graph contains two distinct straight lines, one for each method.

• Trapezoidal Rule: The error line for the trapezoidal rule has a slope of approximately 2. This confirms its theoretical 2nd-order convergence.
• Simpson’s Rule: The error line for Simpson’s rule is significantly steeper, with a slope of approximately 4. This demonstrates its theoretical 4th-order convergence.

A comparison of the results shows that for any given step size $h$, the error from Simpson’s rule is substantially smaller than the error from the trapezoidal rule, highlighting the superior accuracy of the higher-order method for smooth functions.

d) Analysis for a Non-Smooth Function

Finally, the convergence analysis is repeated for all three methods on a new function, $f(x)=\sqrt{|x|^{3}}$, over the interval $[a,b]=[-2,2]$. The exact value for this integral is $I=\frac{16}{5}\sqrt{2}$.

Comments on the Obtained Results

The function $f(x)=\sqrt{|x|^{3}}$ is not sufficiently smooth at the point $x=0$ within the integration interval. While the function and its first derivative are continuous, its second derivative, $f’’(x) \propto |x|^{-1/2}$, is unbounded at $x=0$.

The theoretical error estimates that predict 2nd and 4th-order convergence for these methods are based on the assumption that the function’s higher-order derivatives are continuous and bounded. Since this condition is violated, a degradation in the observed convergence rates is expected.

Book

Matrix Cook Book

Autonomous Driving

• Autonomous Driving
  • Perception
  • Decision-making
    • Architecture
      • Pre-trip decisions
      • In-trip decisions
      • Post-trip decision
  • Speed tracking
    • Dynamic system
      • Open-loop control
      • Closed-loop control
        • Linear controller
    • LTI system
      • Control of LTI system
  • Speed tracking
  • Trajectory tracking
    • Control policy

Perception

• Navigation tool
• Camera
• Radar
• LiDAR
• Infrared detector
• Speedometer
• veh-veh communication
• veh-infrastructure communication

Decision-making

• departure time
• good route
• good lane
• driving style
• way to accel/decel
• make a turn
• park location

Architecture

• Pre-trip

  trip generation, modal choice, routine choice

• In-trip

  Path planning, Maneuver regulation

• Post-trip

  Parking

Pre-trip decisions

• Centralized

  central command center send instructions
  Some win, some lose

• Decentralized

  plan trip independently, selfish
  little communication
  Sometime efficient, sometime inefficient

In-trip decisions

• offline info

  static, in form of map

• online info

  enable the route to be adapted to changing traffic conditions

Post-trip decision

Speed tracking

Dynamic system

• State Variable

  $v[t]$

• State space

  $R>= 0$

• Control input

  $u[t]$(accel)

• System dynamic

  $v[t+dt] = v[t]+u[t]*dt$

Open-loop control

Specify the control input $u[t]$ for all t in $[0,T]$
not a good idea for autonomous driving.

In practice, we need to consider the uncertainty of the system.
We should have:
$$
v[t+dt]=v[t]+u[t]dt+w(t)
$$
Where w(t) is the noise term capturing the uncertainty of the system.

Closed-loop control

Specify the control input $u[t]$ as a function of the state $v[t]$
able to adapt to the uncertainty of the system.
Compare the state $v[t]$ with the desired state $v_desired[t]$
Essentially, we need a mapping from state to control input
like map speed to acceleration
$$
v[t+dt]=v[t]+mu[v[t]]dt+w(t)
$$

Linear controller

the $u[t]$ has linear relationship with $v[t]$
$$
\mu[v] = k[v-v_{desired}]
$$

LTI system

linear time-invariant

Control of LTI system

exponential convergence is stronger than asymptotic convergent
if LTI system is asymptotic convergent, it is also exponential convergent
$$
x[t+1]=ax[t]+bu[t]
$$

Speed tracking

• given:

  $v_{desired}, \delta$

• determine:

  $u[t]$ $t = 0,1,2$

• state

  $v[t]$
  $v[t+1] = v[t] + u[t]\delta + w[t]$

select $u[t]$ so that $w[t]$ will not accumulate

$u[t]=\mu(v[t]) = kv[t]$

$$
v[t+1] = f(v[t],u[t])
$$

Remember the review question

Trajectory tracking

Overview:

• track means asymptotic convergent

• $x[t]$ and $x_{desired}[t]$

• $\lim_{t\rightarrow\infty}|x[t]-x_{desired}[t] | = 0$

• state of system:$[x[t], v[t]]^T$

• if successful:
  $$
  [x[t], v[t]]^T \rightarrow [x_{desired}[t], v_{desired}[t]]^T
  $$

Control policy

• A control policy is a function
• This function maps a state to a control input
  $$
  u[t] = f(x[t],v[t])
  $$

$$
u[t] = -k_1(x[t]-x_{desired}[t])-k_2(v[t]-v_{desired}[t])
$$

$$
k_1,k_2>0
$$