The Iterative Closest Point (ICP) algorithm usually alternates between two steps:

  1. Point correspondence finding
  2. Nonlinear least squares optimization of ICP objective function given those correspondences

In case of depth map alignment, it was found that using projective correspondences instead of nearest neighbours led to better results. If my understanding is correct, then projective correspondences are differentiable, so it should be possible to include projective correspondence search into the ICP objective function.

Let's compare the conventional point-to-plane formulation $E$ with differentiable correspondence search in $\hat E$.

The objective function of point-to-plane ICP (with fixed correspondences):

$$E(R, t) = \sum_i \left(\left(R p_i +t - q_i \right) \cdot n_i\right)^2$$

The objective function of point-to-plane ICP (with correspondence search in objective function):

$$\hat E(R, t) = \sum_i \left(\left(R p_i + t - Q\left(\pi(R p_i + t)\right) \right) \cdot N( \pi(R p_i + t))\right)^2$$


  • $p_i \in \mathbb{R}^3$ is a point of the source point cloud
  • $q_i \in \mathbb{R}^3$ is the point of the target point cloud corresponding to $p_i$
  • $n \in \mathbb{R}^3$ is the normal of $q_i$
  • $Q \in \mathbb{R}^{M \times N \times 3}$ is $M \times N$ vertex map constructed by un-projecting the target depth map
  • $N \in \mathbb{R}^{M \times N \times 3}$ is $M \times N$ normal map corresponding to $Q$
  • $\pi\colon \mathbb{R}^3 \mapsto \mathbb{R}^2$ is the projection function from world space to image plane

As can be seen, the fixed target's vertex and normal correspondences were replaced by queries into the 2D vertex and normal maps.

Has this approach, where a similar formulation of the objective function $\hat E$ was used, been tested before (are there any papers)? If it has, then what are the pros and cons of including projective correspondence search into the ICP objective function? Does it improve convergence? Does $\hat E$ help find better correspondences in later iteration steps than $E$?



Your Answer

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