Linear Least-Squares Point-to-Plane ICP degenerative case

Question

I'm trying to implement Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration Paper describes linear approximation for point to plane distance for rigid ICP. This approach is pretty straightforward however during implementation I faced with some strange degenerative case where such approximation lead to solving singular linear system.

Let me explain. We want to solve following optimization problem (ordinary sum of point-to-plane distances)

$$ argmin_{\alpha, \beta, \gamma, tx, ty, tz}\sum_i((M(\alpha, \beta, \gamma, tx, ty, tz)s_i - d_i)^Tn_i)^2 $$

where $M$ is 4x4 transformation matrix computed from euler angles and translations, $\{s_i\}$ and $\{d_i\}$ are ordered sets of 3d points sampled from surfaces $S$ and $D$, $\{n_i\}$ is ordered set of normals from $D$ which correspond to points $\{d_i\}$

Obviously this problem is non-linear least squares due to trigonometric functions inside $M$. The author proposes linear version of $M$ by replacing $cos(\alpha)$ by 1 and $sin(\alpha)$ by $\alpha$. After this approximation problem above becomes simple linear least squares problem and can be expressed in form

$$ ||Ax - b||_2^2 $$ where $x$ is vector of optimization variables. Matrix $A$ and vector $b$ can be easily expressed from $s_i, d_i, n_i$.

From paper $i$ row of $A$ is equal (by eq 10 from paper) $$ a_i = [n_{iz}s_{iy} - n_{iy}s_{iz}, n_{ix}s_{iz} - n_{iz}s_{ix}, n_{iy}s_{ix} - n_{ix}s_{iy}, n_{ix}, n_{iy}, n_{iz}] $$

Now let consider simple geometric example. Let $\{s_i\} = \{[1, 0, 0], [0, 1, 0], [0, 0, 1], [-1, 0, 0], [0, -1, 0], [0, 0, -1]\}$ and ${d_i} = {n_i} = {s_i}$. Now vertices $s_i$ are parallel with normals $n_i$.

Obviously first 3 columns of $A$ are equal zero which leads us to ill-posed least squares problem.

My question is: What is geometrical interpretation for such case? Why collinearity of normals and points leads to bad optimization problem?

Sory for my English/Latex skills

Answer 1

This approach is the result of a Taylor approximation, which assumes that we are not very far from the solution (solution is the case when $R\approx I$ - no more updates can be made). Under large rotations this linearization will fail.

Moreover, point to plane distance is prone to sliding errors when normals and points have particular configurations, such as the point lying on the plane. In these cases it is not the approximation, but the metric that suffers.

Having said that, there is a MATLAB code, as well as an OpenCV module implementing this paper. They also suffer from such drawback when the rotation is far apart.