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


1 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.

  • $\begingroup$ Thank you for answer! Unfortunately configuration in my question will broke algorithm even for zero rotation (matrix "A" does not depends on rotation). $\endgroup$
    – Daiver
    Commented Jan 5, 2018 at 13:05

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.