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

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

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.