Generalized Iterative Closest Point (GICP)

stateEstimation worldModeling A form of Iterative Closest Point (ICP) that models each point as a gaussian distribution based on its local neighborhood.

How it works

Instead of computing the distance error between each point, we calculate the extension of a Mahalanobis Distance between two a local distributions in the source and target.

Distance between two distributions

People colloquially call this computation a Mahalanobis Distance even though its not the formal definition. More of an extension.

Given two points, that we correspond to be two local distributions.

$${\mathbf{p}}_i\sim \mathcal{N}({\mathbf{\mu }}_i,{\mathbf{\Sigma }}_i)$$ $${\mathbf{q}}_j\sim \mathcal{N}({\mathbf{\mu }}_j,{\mathbf{\Sigma }}_j)$$

Then the extended mahalanobis distance between them is

$$D_M^2({\mathbf{\mu }}_i,{\mathbf{\mu }}_j)=({\mathbf{\mu }}_i-{\mathbf{\mu }}_j{)}^T({\mathbf{\Sigma }}_i+{\mathbf{\Sigma }}_j{)}^{-1}({\mathbf{\mu }}_i-{\mathbf{\mu }}_j)$$

This no longer computes the number of standard deviations two points are from each other.

Instead, its better to think of this as a likelihood the two points are the same.

Probabilistic Interpretation of GICP

Say we have two local distributions. One is in the source cloud, another is in a target cloud.

$$p_i={\mu }_{p_i}+{ϵ}_{p_i}\sim \mathcal{N}({\mu }_{p_i},{\Sigma }_{p_i})$$ $$q_i={\mu }_{q_i}+{ϵ}_{q_i}\sim \mathcal{N}({\mu }_{q_i},{\Sigma }_{q_i})$$

These distributions are found by a k cluster around the points.

We then model the difference between the two points as.

$$\Delta =p_i-q_i$$

Assumption: They are the same point

If we assume them to share the same true point, then we would witness the difference to have a mean of 0 and variance equivalent to the sum of the variances of the two distributions.

$$p_i=x^{\ast }+{ϵ}_{p_i},{ϵ}_{p_i}\sim \mathcal{N}(0,{\Sigma }_{p_i})$$ $$q_i=x^{\ast }+{ϵ}_{q_i},{ϵ}_{q_i}\sim \mathcal{N}(0,{\Sigma }_{q_i})$$

where $x^{\ast }$ is the true value of the point.

$${\Delta }_i=0+{ϵ}_{p_i}-{ϵ}_{q_i}\sim N(0,{\Sigma }_{p_i}+{\Sigma }_{q_i})$$

This tells us that if the two points share the same true point, then their difference can be modeled by a Gaussian distribution with a $\mu =0$ and $\Sigma ={\Sigma }_{p_i}+{\Sigma }_{q_i}$.

Which means that we know that we’ve successfully transformed the source cloud to the target when the likelihood of $N(0,{\Sigma }_{p_i}+{\Sigma }_{q_i})$ is at its max

What we are trying to find

Because the goal of GICP is to find a transform $T$ such that

$$T{p}_i=q_i$$ $$T{p}_i=R{\mu }_{p_i}+t+{ϵ}_{p_i}\sim \mathcal{N}(R{\mu }_{p_i}+t,R{\Sigma }_{p_i}{R}^T)$$

We must find a transform on the source cloud that maximizes our Likelihood that the observed differences can be modeled by $N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i})$

$$T={\text{argmax}}_T\mathcal{L}(T|\Delta )={\text{argmax}}_T\prod _{i=1}^n\mathcal{L}(N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i})|{\Delta }_i)$$

Where

$$\mathcal{L}(N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i})|{\Delta }_i)\propto p({\Delta }_i|N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}))$$ $$p({\Delta }_i|N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}))=\frac{1}{(2\pi {)}^{3/2}(|R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}|{)}^{1/2}}\exp \left(-\frac{1}{2}{\Delta }_i^T(R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}{)}^{-1}{\Delta }_i\right)$$ $$\text{where}{\Delta }_i=R{\mu }_{p_i}+t-{\mu }_{q_i}$$

Which is just the observation when put inside a gaussian distribution with mean 0 (since we are calculating how likely we can model the observation (with the transform) as part of a distribution assuming that the source and target share the same point.)

Because $\mathcal{L}(T|\Delta )$ is very expensive to calculate as a product, we can derive the log-likelihood to make it a summation, which is alot easier to optimize.

$$T={\text{argmax}}_T\log \mathcal{L}(T|\Delta )={\text{argmax}}_T\log \prod _{i=1}^n\mathcal{L}(N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i})|{\Delta }_i)$$ $$\log \mathcal{L}(T|\Delta )=\sum _{i=1}^n\log \mathcal{L}(N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i})|{\Delta }_i)$$

Taking the log of the probability…

$$logp({\Delta }_i|N(0,R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}))$$ $$=\log \left(\frac{1}{(2\pi {)}^{3/2}(|R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}|{)}^{1/2}}\exp \left(-\frac{1}{2}{\Delta }_i^T(R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}{)}^{-1}{\Delta }_i\right)\right)$$ $$-\log \mathcal{L}(T)=\sum _{i=1}^n\left[\frac{3}{2}\log (2\pi )+\frac{1}{2}\log |R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}|+\frac{1}{2}{\Delta }_i^T(R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}{)}^{-1}{\Delta }_i\right]$$

We can drop the constants because they don’t depend on T, and GICP often drops the determinant term $\frac{1}{2}\log |R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}|$ as well

$$-\log \mathcal{L}(T)=\sum _{i=1}^n\left[\frac{1}{2}{d}_i^2(T)\right],\text{where}{d}_i^2(T)={\Delta }_i^T(R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}{)}^{-1}{\Delta }_i$$

Which is where we finally end up getting the mahalanobis-like distance calculation for GICP.

all this to tell us that we are actually calculating the likelihood that the two points are the same true point.

How do we actually get this transformation

We want to solve

$$T^{\ast }=\arg \underset{T}{\min }\sum _{i=1}^n{d}_i^2(T)$$ $$d_i^2(T)={\Delta }_i^T(R{\Sigma }_{p_i}{R}^T+{\Sigma }_{q_i}{)}^{-1}{\Delta }_i$$

This is a non-linear least squares problem, which can be solved by a class of iterative optimizers. There are many ways to solve for the transformation here. One way is Gauss-Newton Method

There are other methods, but I can’t be bothered to go any further into this for now. small_gicp is fast not because of its optimizer, but rather because of its parallel processing architecture and a bunch of implementation optimizations.

Surprise! Underthehood, we are actually calculating the Mahalanobis Distance with Lie Algebra.