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

Preview:

Citation preview

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

University of Hamburg

MIN FacultyDepartment of Informatics

Brief introduction to registration 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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◮ . . .

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration 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

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration - ICP & color ICP 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

ICP Visualization

◮ dark green: modelscan

◮ yellow : data scan◮ bright green:

point-to-pointcorrespondences

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Point reduction

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

points).

University of Hamburg

MIN FacultyDepartment of Informatics

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

kd-trees

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

points.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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]

University of Hamburg

MIN FacultyDepartment of Informatics

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

Results from ICP

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

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration - Normal Distribution Transforms 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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)

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

Color NDT

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration - Plane-based scan matching 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Algorithm overview

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

planar patches extraction

University of Hamburg

MIN FacultyDepartment of Informatics

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

Plane correspondence determination

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

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration - Depth-interpolated local image features 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Algorithm overview

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

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.

University of Hamburg

MIN FacultyDepartment of Informatics

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

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Finding the rigid transformation based on ICP

Rewrite the second step of ICP

J(R, t) =N

∑i=1

‖pci −Rpp

i − t‖2

University of Hamburg

MIN FacultyDepartment of Informatics

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

Data collection

University of Hamburg

MIN FacultyDepartment of Informatics

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

Results

University of Hamburg

MIN FacultyDepartment of Informatics

Ralated work on registration - Visual /laser & inertial senseing 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Algorithm overview

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

University of Hamburg

MIN FacultyDepartment of Informatics

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

Algorithm data flow

University of Hamburg

MIN FacultyDepartment of Informatics

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

Rotate to local vertical and magnetic north

University of Hamburg

MIN FacultyDepartment of Informatics

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

Translation from risual correspondence

University of Hamburg

MIN FacultyDepartment of Informatics

Overview of this presentation 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

University of Hamburg

MIN FacultyDepartment of Informatics

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

overview of the content

University of Hamburg

MIN FacultyDepartment of Informatics

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

Thank you!Any questions and suggestions are welcome!

Recommended