ICP explained
2024-08-17 (Last updated )Check out the visualization I made in the Projects page.
I'll be explaining the math and possible optimizations you can make with your implementation. Sorry about the inconsistency of the fonts too. It's not the same as using the $$\LaTeX$$ bot on Discord. I haven't finished it but I'll try to work on it during my free time.
Background (things you probably should know first)
TODO: explain here
Formalizing the Problem
Let's suppose we're given the original scene below:
We want to estimate the pose of the block M in the scene.
We do this by fitting the 3D point clouds of the object in the scene and try to fit our 3D point clouds from the original model we know to the cluster of point clouds of the object in the scene.
We do this by iteratively solving the following problem:
$$\begin{gather} \min_{R, t, c} \ \ \ \sum_{i} | R s_i + t - m_{c_i}|_2^2 \\ s.t. \ \ \ R^T = R^{-1}, \mathrm{det}(R) = 1 \ \end{gather}$$
where $$s_i$$ are the points in the scene point cloud, $$m_i$$ are the points in the model point cloud, and $$c_i$$ is the index of the point in the $$m$$ that corresponds most closely with the $$i$$-th point in the scene
This is a somewhat hard and non-convex problem, so instead of solving the optimization in one step, the algorithm alternates between solving for $${\ R,t} \text{ and }c$$ seperately.
But there are a few problems with brute force solving this problem. that's a pretty slow to brute force-through, especially when you can have very dense 3D point clouds.
- Our error function grows quadratically
- It's sensitive to outliers
- It's generally a relatively slow algorithm.
- There are point clouds in the raw data that are not relevant to the object we want to localize.
KD Trees + Closest Point Heuristic
Zhang et al. proposes a solution: why not use a modified version of KD-trees?TODO: explain why we're using KD trees
The nice thing about this approach is that it's robust to occulusions and outliers. Conveniently, both SciPy and Open3D have KD-trees as an available data structure after importing the library, so I decided to use KD trees.