Wednesday, May 25, 2016

ICP Algorithm for Point Cloud Stitching

ICP stands for Iterative Closest Point algorithm. It is a well-known algorithm used to align two point clouds.

Iterative Closest Point


  1. Search for correspondences.
  2. Reject bad correspondences.
  3. Estimate a transformation using the good correspondences.
  4. Iterate.

Many tutorials are available online for ICP algorithm such as http://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf

The popular PCL library for point cloud processing provides ICP class here: http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

Here is another example of Matlab implementation for ICP: http://www.csse.uwa.edu.au/~ajmal/code/icp.m

Despite the popularity of ICP, when using it, I find that the result is not always satisfactory. The biggest problem is that there is no guarantee that ICP will converge to global optimum. In practice, it often converges to local optimum. As shown in the picture below, when point clouds A and B merge, depending on the starting positions, it is possible that ICP only merges part of the clouds.


Tuesday, May 24, 2016

Use Rodrigues' Rotation to Convert 3D data to 2D

In this blog, we introduce how Rodtrigues' rotation formula can be use to convert 3D data to 2D. Let us assume that we have a bunch of 3D points, and we want to project them to x/y axis plane for easier analysis. The Matlab code below implements this function.

clear all; close all;

% collect 3D points 
xyz = [-0.485 0.130 1.489
    0.008 0.134 1.464
    -0.008 0.425 1.458
    -0.465 0.414 1.470
    -0.489 0.136 1.483
    -0.465 0.412 1.470
    0.006 0.137 1.464];

% Model them as a plane in 3D using least square method
[h,l]=size(xyz);
coef = inv(xyz'*xyz)*(xyz'*ones(h,1));
coef_unit = (coef/sqrt(coef'*coef)).';

% Rodrigues' rotation formula
in0 = coef_unit;
in1 = [0 0 1];
% Reference
in0Norm = in0 ./ sqrt(in0*in0.');
in1Norm = in1 ./ sqrt(in1*in1.');
axisRaw = cross(in0Norm, in1Norm);
axis = axisRaw./sqrt(axisRaw*axisRaw.');
angle = acos(dot(in0Norm, in1Norm));
M = [axis(1)^2+(1-axis(1)^2)*cos(angle) axis(1)*axis(2)*(1-cos(angle))-axis(3)*sin(angle) axis(1)*axis(3)*(1-cos(angle))+axis(2)*sin(angle);
axis(1)*axis(2)*(1-cos(angle))+axis(3)*sin(angle) axis(2)^2+(1-axis(2)^2)*cos(angle) axis(2)*axis(3)*(1-cos(angle))-axis(1)*sin(angle);
axis(1)*axis(3)*(1-cos(angle))-axis(2)*sin(angle) axis(2)*axis(3)*(1-cos(angle))+axis(1)*sin(angle) axis(3)^2+(1-axis(3)^2)*cos(angle)];
    
% Project 3D points to 2D plane with z=0 
xyz_ortho = xyz - xyz*coef_unit.'*coef_unit;   
xyz_prj = M*xyz_ortho.';

Let us walk through the code step-by-step.

Step 1: A plane in 3D space can be characterized as \( a x + b y + c z = 1 \). Assuming that matrix \(X\) has all 3D points: \[ X = \begin{bmatrix}
    x_{0}       & y_{0} & z_{0}  \\
    x_{1}       & y_{1} & z_{1} \\
    ...              & ...        &...\\
    x_{N}       & y_{N} & z_{N}
\end{bmatrix}\]
Then least square estimator for [a b c] is \( coef = (X'X)^{-1}(X'h)\) where \(h = [1 1 ... 1]'\) and \(X'\) is the transpose of \(X\). Normalized version of coef is called coef_unit.

Step 2: Use Rodrigues' rotation formula to find the matrix \(M\) which can rotate coef_unit to [0 0 1].

Step 3: First we project all 3D points to a plane characterized by [x y z]*coef_unit = 0. This is achieved by:
xyz_ortho = xyz - xyz*coef_unit.'*coef_unit;   
The last step is to use rotation matrix \(M\) to reduce the dimension of xyz_ortho to 2D. After the rotation, all z-axis elements in xyz_prj are 0's.




Monday, May 23, 2016

Vector Kalman Filter

Kalman filter is one type of statistical filtering. It was invented in 1970's and has been widely used till now. There are different variations of Kalman. We focus on vector Kalman filter here.

For system model, we have two equations:
\[s[n]=\textbf{A}s[n-1]+\textbf{B}u[n] \qquad (1)\]
\[x[n]=\textbf{H}[n]s[n]+w[n] \qquad (2)\] 
\(s[n], x[n], u[n], w[n]\) are vectors; \(\textbf{A}, \textbf{B}, \textbf{H}[n]\) are matrices. \(s[n]\) is the signal and what we want to detect. Equation (1) shows that the signal in time n and time n-1 are correlated. \(s[n]\) is generated by \(s[n-1]\) plus a noise \(u[n]\). \(u[n]\) is white noise with \(N(0, \textbf{Q})\). But equation (1) alone does not give a good estimate of \(s[n]\). As time goes by, \(u[n]\) keeps adding noise to the estimate and eventually will render it unusable. That is where equation (2) comes into the picture. \(x[n]\) in equation (2) is observation. In each moment, the observation of \(x[n]\) is used to refine the estimate of \(s[n]\). \(x[n]\) is also corruptedby noise \(w[n]\), which has spectral density of \(N(0,\textbf{C}[n])\).

Now the problem comes: how should we utilize the observation? If we just have equation (2) but not equation (1), then MMSE or LS estimators are natural choices. But with equation (1), these estimators will be sub-optimal since they fail to consider the previous results of s. Kalman filter was invented to fully utilize the information given in both equations. Under Kalman filter, the estimate of s, s_hat, is derived from equation (1) but then corrected by the observation in equation (2).

The first step of Kalman filter is prediction:
\[\hat{s}[n|n-1]=\textbf{A}\hat{s}[n-1|n-1]\qquad(3)\]
The new MSE of \(\hat{s}[n|n-1]\) becomes
\[\textbf{M}[n|n-1]=\textbf{AM}[n-1|n-1]\textbf{A}'+\textbf{BQB}'\qquad(4)\]
where \(\textbf{A}\)' is the transpose of conjugate of the matrix \(\textbf{A}\). Equation (4) shows that without correction, MSE will keep growing due to \(\textbf{BQB}'\).

For correlation, we uses MMSE filter, which is the best known filter to reduce Mean Square Error. The formula for MMSE filter is
\[\textbf{K}[n]=E(x[n]s[n]')(E(x[n]x[n]')^{-1}\qquad(5)\]
where \((.)^{-1}\) means matrix inverse. Thus
\[\textbf{K}[n]=\textbf{M}[n|n-1]\textbf{H}[n]'(\textbf{H}[n]\textbf{M}[n|n-1]\textbf{H}[n]'+\textbf{C}[n])^{-1}\qquad(6)\]

Then the correction formula for Kalman becomes
\[\hat{s}[n|n]=\hat{s}[n|n-1]+\textbf{K}[n](x[n]-\textbf{H}[n]\hat{s}[n|n-1])\qquad(7)\]
\(x[n]\) is supposed to be a good estimation of \(\textbf{H}[n]\hat{s}[n|n-1]\). The difference between these two are used to correct \(\hat{s}\).
Correction should reduce MSE. The new MSE is
\[E\big((s[n]-\textbf{K}[n]x[n])(s[n]-\textbf{K}[n]x[n])'\big)\]
Based on the orthogonality rule of MMSE filter,
\[E((s[n]-\textbf{K}[n]x[n])x[n]')=0\]
Therefore, MSE can be modified to be

E((s[n]-K[n]x[n])(s[n]-K[n]x[n])')
=E((s[n]-K[n]x[n])s[n]')
=E(s[n]s[n]')-K[n]E(x[n]s[n]')
= (I-K[n]H[n])M[n|n-1]
=M[n|n]

I don't want to drive people to sleep but this is how things are. Kalman filtering is not easy. To use Kalman, you need to come out with the models of A, B, C, Q, H. Depending on the size of s and x, you may also need to inverse a large matrix. 

Steven Kay's classic book: Fundamentals of statistical signal processing -- estimation theory, has one chapter dedicated to Kalman filter. You can refer to that book for more knowledge of Kalman filtering.