0:05

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.

Â 2:20

Our goal is to put the newly measured points into the right place on

Â the map model.

Â In order to do so, we need to find a rotation and

Â translation that move the measured points to match the model points.

Â Additionally, we need to know which measurement points correspond to

Â which points in the model.

Â We can visually detect the corresponding parts in this example.

Â 2:49

However, for a robot with tens of thousands of noisy points it

Â is not obvious to see the possible matching pattern.

Â Now let's look at how ICP handles these

Â two problems in an expectation maximization framework.

Â The strategy of the ICP algorithm takes an optimistic

Â assumption that the point sets are close enough.

Â In this way we have a good prior of the rotation r and translation, t.

Â Under this assumption,

Â the correspondence of a point will be the closest point to that one.

Â In this way,

Â we will find closest points of all measured points corresponding to the map.

Â When a map has a tree structure,

Â this process is much faster than a brute force search.

Â Once we have our correspondences, we can enhance our estimate of R and

Â t, by solving this optimization.

Â If you are interested,

Â the cited paper gives details of the solution in order to obtain R and t.

Â It's good practice.

Â 4:00

After each iteration We will have better and

Â better correspondences in addition to better registrations.

Â We iterate this until it converges.

Â Once it does, we can obtain the rotation and

Â translation between the two sets of points.

Â Let me show you an example with two sequential point clouds obtained from

Â a depth camera while a quadrotor is flying in a room.

Â Since the robot was moving in the yellow direction,

Â the scene from the body looks rotated around the z axis.

Â Now, we want to register the two sets to become a single, consistent point cloud.

Â This short animation shows how the ICP algorithm

Â converges to a local minimum after an iteration.

Â Through registration, we are computing the motion increment of the robot.

Â As we did in scan matching in the previous lecture, this gives a reference

Â location for the robot with respect to points that are known.

Â Although we need a pretty close initial alignment this algorithm

Â is widely used in many robotic applications,

Â including three dimensional simultaneous localization and mapping.

Â We have seen how the ICP algorithm could be used for localization with 3D sensors.

Â There are many variants of this algorithm according to

Â different optimization formulas different ways to choose correspondences and

Â different ways to reject bad points.

Â In general this has been a good overview of many different localization techniques.

Â