Previously, we have seen a localization method based on

particles that have a three-dimensional state of x, y, and eo.

This captures a ground robot.

However, when not constrained to the ground,

a post has six degrees of freedom which requires exponentially more

points to be sampled in order to produce reasonable registration performance.

Instead of relying on particles, we can use a direct optimization

to find the registration between our measurements and the map.

After reviewing what we have learned in the previous weeks,

I will introduce the ICP, Iterative Closest Point algorithm,

as well as odometry for three dimensional localization.

Let us start with a brief review of the expectation maximization

algorithm from week one.

We have seen that this algorithm is useful for complicated optimizations,

such as the gaussian mixture model parameter estimation problem.

With the introduction of an initial guess coupled with the latent variable,

usually expressing membership.

We are able to obtain a local optimal solution for a given problem.

We will shortly see that the iterative closest point algorithm

works in the same fashion.

What we learned in week 3 is as follows.

First we observe a 3D point cloud from 3D sensors.

This figure shows an example from a depth camera.

A 3D map is usually represented as a tree structure instead of as a full grid

as done in two dimensions.

This is in order to have efficient means.

The map can keep the full precision of point location.

Due to the special organization, we can speed up the finding

of the closest point to a given point in each map update.

Now we will look at this in detail, we have two sets of points,

one of them is a point cloud as a measurement and

the other is a point cloud of the map model.