Registration using laser range finder as the premary sensor · Brief introduction to registration Registration using laser range ﬁnder as the premary sensor Deﬁnition of registration

University of Hamburg

MIN FacultyDepartment of Informatics

Registration using laser range finder as the premary sensor

Registration using laser range finder as thepremary sensor

XIAO Junhao

Department Informatics, Group TAMS

May 27, 2010



Outline Registration using laser range finder as the premary sensor

Outline

Brief introduction to registrationRalated work on registration

ICP & color ICPNormal Distribution TransformsPlane-based scan matchingDepth-interpolated local image featuresVisual /laser & inertial senseing

Overview of this presentation



Brief introduction to registration Registration using laser range finder as the premary sensor

Outline







Definition of registration

Registration is the problem of matching scans to build acomplete model when the exact pose of the scans is unknown.Given two overlapping scans and an initial guess for atransformation that will bring one scan (the source) to thecorrect pose in the coordinate system of the other scan (thetarget), the output is a refined transformation.




Motivation

Motivation for registration in the mobile roboticscommunity

◮ 3D world model is significant for mobile robot◮ Multi frames of sensor data are needed for modelling◮ It is impossible to know the exact pose of the scanner◮ Error from odometry accumulates especially fast◮ Scan matching is considered a good way




Algorithms for 3D scan matching

There are many algorithms for 3D scan matching◮ Iterative Closest Point (ICP)◮ Normal Distributions Transform (NDT)◮ Plane-based scan matching◮ Depth-interpolated image features (DIFt)◮ Registration using vision/laser and inertial sensing◮ Gaussian fields◮ Point-based probabilistic registration◮ Likelihood-field matching◮ . . .



Ralated work on registration Registration using laser range finder as the premary sensor

Outline






Ralated work on registration - ICP & color ICP Registration using laser range finder as the premary sensor

Outline







Algorithm overview

ICP was proposed for 2D point clouds alignment in 1992. Forprior point set M (“modle set”) and data set D

Step◮ 1. Find the corresponding point pairs in M and D

◮ 2. Calculates the transformation (R, t) for minimizingequation

E(R, t) =Nm

∑i=1

Nd

∑j=1

ωi,j‖m̂i − (Rd̂j + t)‖2

◮ 3. Iterate 1. and 2.




ICP Visualization

◮ dark green: modelscan

◮ yellow : data scan◮ bright green:

point-to-pointcorrespondences




Problems in ICP

There are two main problems with ICP◮ It is a point-based method that does not consider the local

shape of the surface around each point◮ The nearest-neighbor search in the algorithm’s central loop

is computationally expensive




Accelareta methods for ICP

The following issues can be used to accelerate computation ofclosest points:◮ Point reduction◮ kd-trees

◮ Approximate kd-tree search◮ Cached kd-tree search

◮ Parallelization algorithm




Point reduction

Left: a view of a 3D scene 66785 3D data points. Right: subsampled version (points have been enlarged, 6700 data

points).




kd-trees

Left: Two 3D point clouds. Middle: Octree corresponding to the black point cloud. Right: Octree based on the blue

points.




Approximate kd-tree search

◮ Consider the query point pq

◮ The approximate search isdiscontinued if the distanceto the unanalyzed leaves islarger than

‖pq −p‖/(1+ ε)

◮ Approximate closest point isthe input for ICP




Cached kd-tree searchRecall ICP:◮ 1. Find the corresponding point pairs in M and D

◮ 2. Calculates the transformation (R, t) for minimizingequation

E(R, t) =Nm

∑i=1

Nd

∑j=1

ωi,j‖m̂i − (Rd̂j + t)‖2

◮ 3. Iterate 1. and 2.

Idea: Make use of the iterative structure of the algorithm◮ Save closest Points in a vector v◮ In addition, save a pointer to the leaf, where the closest

point has been found◮ The kd-tree contains pointers to predecessor nodes




Parallelization algorithm

Algorithm steps◮ Divide the data set D into p parts and to send this part

together with the whole model set M to the child processesthat compute concurrently the expensive closest pointqueries

◮ Points correspondences are transmitted back to the parent,that uses it for computing the transformation

◮ The transformation is sent to the childs, which transform thedata set D

◮ The process is iterated until the minimum is reached




Color ICP

Color information is used when search closest point, the 6Ddistance between two points is defined:

d6(p1,p2) = [(x11− x21)2 +(x12− x22)

2 +(x13− x23)2+

α1(c11− c21)2 + α2(c12− c22)

2 + α3(c13− x23)2]




Results from ICP

Left: Registration based on odometry only. Right: Model based on incremental matching right before closing the loop.



Ralated work on registration - Normal Distribution Transforms Registration using laser range finder as the premary sensor

Outline







Algorithm overview

Normal Distributions Transform was proposed for 2D scanmatching in 2003.

Key IdeaIndividual points =⇒ Combination of normal distributions.

AssumptionThe locations of the reference scan surface points weregenerated by a D-dimensional normal random process.




NDT representation (1)

First:Subdivide the space into a grid of cells.Second:Calculate mean vector q and covariance matrix C foreach cell:

q =1n

n

∑k=1

xk, C =1

n−1

n

∑k=1

(xk −q)(xk −q)T

Result:The probability a point at position x in cell b can becalculated:

p(x) =1c

exp(−(x−q)T C−1(x−q)

2)




NDT representation (2)

3D-NDT surface representation for a tunnel section.Brighter, denser parts represent higher probabilities. The cells

have a side length of 1m.




NDT scan registration

problem formulation

ψ =n

∏k=1

p(T(−→p ,−→x k))

for optimization, rewrite it as

− logψ = −n

∑k=1

logp(T(−→p ,−→x k))

which can be solved by Newton’s algorithm.




Choose cell size

Too largeLess precise registration

Too smallGood initialization needed.Cells with low point densitycan’t be used.

solutions◮ Fixed discretization◮ Octree discretisation◮ Iterative discretisation◮ Adaptive clustering◮ Linked cells

Martin Magnusson, Tom Duckett and Achim J. Lilienthal, 3D Scan Registration for Autonomous Mining Vehicles.

Journal of Field Robotics, 24:10, 24 Oct 2007, pp.803-827.




Color NDT



Ralated work on registration - Plane-based scan matching Registration using laser range finder as the premary sensor

Outline







Algorithm overview

Step◮ 1. Planar patches extraction◮ 2. Plane correspondence determination◮ 3. finding the optimal decoupled rotations and translations




planar patches extraction




Plane correspondence determination

◮ Plane Registration based on Rotation of a Unit Sphere(PRRUS)



Ralated work on registration - Depth-interpolated local image features Registration using laser range finder as the premary sensor

Outline







Algorithm overview

Step◮ 1. SIFT feature extraction◮ 2. Detecting visual correspondences◮ 3. Estimating visual feature depth◮ 4. Finding the rigid transformation based on ICP




Detecting visual correspondences

Feature description

fi = {[X,Y]i,Hi}

Feature matchingCalculates the Euclidean distance between each feature inimage Ia and all the features in image Ib.




Estimating visual feature depth

◮ ×: an image feature◮ Circles: range readings◮ P0..M : range readings used

to compute the covarianceestimate

◮ P0: laser point whichdetermins the depth of thefeature




Finding the rigid transformation based on ICP

Rewrite the second step of ICP

J(R, t) =N

∑i=1

‖pci −Rpp

i − t‖2




Data collection




Results



Ralated work on registration - Visual /laser & inertial senseing Registration using laser range finder as the premary sensor

Outline







Algorithm overview

Step◮ 1.Rotate point cloud to local vertical and magnetic north◮ 2.SIFT feature extraction◮ 3.Translation from visual correspondence




Algorithm data flow




Rotate to local vertical and magnetic north




Translation from risual correspondence



Overview of this presentation Registration using laser range finder as the premary sensor

Outline







overview of the content




Thank you!Any questions and suggestions are welcome!

Documents

Registration using laser range finder as the premary sensor · Brief introduction to registration Registration using laser range ﬁnder as the premary sensor Deﬁnition of registration