3d mapping and openrdk

INDUSTRIALESETSII | UPM

Paloma de la Puente - [email protected]://intelligentcontrol.es/palomaOpenRDK WorkshopRome, March 16th-19th 2009

DISAM-UPM

Paloma de la Puente. DISAM-UPM 2

Outline of the presentation

1. Introduction

2. 3D Feature Based Mapping

2.1. 3D Data Acquisition

2.2. Segmentation

2.3. Geometric Model

2.4. Localization and Mapping

3. Integration within the OpenRDK framework

4. Experiments

5. Conclusions

• Modeling environments with 3D feature-based representations is a challenging issue in current mobile robotics

- Safe navigation- More realistic models- Increased interpretability


1. Introduction

1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Robotic Platform: robot Pioneer 3AT

• Laser scanner SICK LMS200

• Amtec Robotics PowerCube70 pan/tilt unit

NEMO

1. Introduction

2. 3D Feature Based Mapping2.1. 3D Data Acquisition

3. Integration

4. Experiments

5. Conclusions



•System configuration: distribution schema

Client

Data Processing and mapping

algorithms

Data Server

Server computer

Serial Port. 422 interface

TCP Streams

LMS 200

Pioneer 3 AT

PW70

Serial Port. RS232 interface

Serial Port. RS232 interface

500 kb1. Introduction


3. Integration

4. Experiments

5. Conclusions



•3D points organized in a matrix structure

- Rows: individual 2D scans corresponding to increasing values of the tilt angle

- Columns: cardinality of a certain measurement within its 2D scan

1 2

1 2

1 2

n

n

n

p p p

p p p

p p p

tilt = min_angle

tilt = max_angle

.

.

.

2Dscan 1

2Dscan 2...

2Dscan m

• Relative transformations to obtain the 3D points’ cartesian coordinates are applied by means of homogeneity matrices

n=181 tilt

1. Introduction


3. Integration

4. Experiments

5. Conclusions



1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.2. Segmentation

• Segmentation undertakes the partition of a 3D point cloud into smaller subsets representing objects of interest

•Different approaches

- Edge-based segmentation

- Region-based algorithms

- Scanline-based segmentation

- Hybrid solutions

1. Introduction

2. 3D Feature Based Mapping2.2. Segmentation

3. Integration

4. Experiments

5. Conclusions


2.2. Segmentation

• Edge-based segmentation

- Fast

- Direct and natural

- Requires detection of both jump and crease edges

- More noise sensitive

Crease edgesJump edges

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.2. Segmentation

• Novel edge-based algorithm to solve the segmentation problem by integrating computer vision techniques

• Key idea to create the image: the residuals of the plane fitting on a point’s neighborhood gives an approximation of the local surface curvature

di

dj

n

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.2. Segmentation

Computation of the residual of each point’s neighborhood least squares

fitting to a plane

Creation of a range image structure with the obtained values

Closing x 2+

Binary Thresholding

Floodfilling algorithm to give different colors to different regions in the

image

Classification of the points according to the corresponding pixel’s color

OpenCV libraries

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.2. Segmentation

• Subdivision of regions

- Enhanced robustness

- Still very fast1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Least Squares Plane Fitting

- Hessian Normal Form zyx oooo ,,

zyx nnnn ,,

tt t N

i t

iN

i

N

i t

i

t

izyx N

z

N

y

N

xoooo

11 1

,,,,

t points , ,i i i iN p x y z

N

i i

N

i ii

N

i ii

N

i ii

N

i i

N

i ii

N

i ii

N

i ii

N

i i

zzyzx

zyyyx

zxyxx

A

0

2

00

00

2

0

000

2

~~~~~

~~~~~

~~~~~

ziyixiiiii ozoyoxzyxp ,,~,~,~~

n

Eigenvector associated to the smallest eigenvalue

1. Introduction

2. 3D Feature Based Mapping2.3. Geometrical Model

3. Integration

4. Experiments

5. Conclusions



• Representation of the local reference frame attached to a plane

•Orientation given by the Euler angles roll (Φ), pitch (θ) and yaw (ψ)

atan2( , )

acos( )

0

y x

z

n n

n

x

y

z

o

o

ox

x

z

z

yx

y

C S - S C

0 0 0 1

x

y

z

C C S C S C S S o

S C S S S C C S S C C S oH

S C S S C o

1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Convex-hull based representation to characterize the plane patches

•Region filtering

CGAL libraries

1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Maximum Probability EKF plane based algorithm

• Representation symmetries

0 0 1 0 0 0

0 0 0 0 1 0

0 0 0 0 0 1

B

1. Introduction

2. 3D Feature Based Mapping2.4. Localization and Mapping

3. Integration

4. Experiments

5. Conclusions



• Prediction stage

11ˆ , ˆ( , )

k R k kkR R x u Rx f x u x u

Rx Robot’s location in the global reference system

ˆ~ ( , )

x

y

z

u

u

uu N u Q

u

u

u

, , 0zu u u when operating on flat terrain

1ˆ T T

k x k x u uP F P F F QF

6D composition of relative transformations

, 11ˆ 1 ˆ( , )R kk

x x u RR

fF J x u

x

, 11ˆ 2 ˆ( , )R kk

u x u R

fF J x u

u

1 2,J J Jacobian matrices of the composition of relative transformations with respect to the first and second components

1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Data Association

( )j iij F R Oh B x x x

Rx Robot’s location in the global reference system

2 ´ 1 ´( , ) ( , )ij j i i

ijx F R O R O

R

hH BJ x x x J x x

x

jFx Location of the jth planar

patch in the map

iOx Location of the ith observed

planar patch (relative to the robot’s pose)

6D composition of relative transformations

6D inversion of relative transformations

2 ´ 2 ´( , ) ( , )ij j i i

ijz F R O R O

R

hH BJ x x x J x x

x

ij ij ij ij

T Tij x k x z i zS H P H H R H 2 1T

M ij ijd h S h

dM = Mahalanobis distance

If the minimum value is small enough the association is accepted

iR Covariance matrix of the noise of the plane extraction process

1. Introduction


3. Integration

4. Experiments

5. Conclusions



• Correction stage

min

min

min

1

2

k

t

h

hh

h

1

2

k

x

xx

xt

H

HH

H

1

2

k

z

zz

zt

H

HH

H

k k k k

T Tk x k x z zS H P H H RH

1

2

t

R

RR

R

1

k

Tk k x kW P H S

ˆk kR R k kx x W h

6 6 1( )kk x k x kP I W H P

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.4. Integration within the OpenRDK framework

• Nemo’s USARSim Model

1. Introduction


3. Integration

4. Experiments

5. Conclusions

UsarBot.Ini

[USARBot.NemoP2AT]

JointParts=(PartName="ScannerSides",PartClass=class'USARModels.Kurt3DScannerSides',DrawScale3D=(X=1.0,Y=0.4,Z=1.0),bSteeringLocked=true,bSuspensionLocked=true,BrakeTorque=100.0,Parent="",JointClass=class'KCarWheelJoint',ParentPos=(X=0.16,Y=0.0095,Z=-0.16), ParentAxis=(Z=1.0),ParentAxis2=(Y=1.0),SelfPos=(Z=0.0),SelfAxis=(Z=1.0),SelfAxis2=(Y=1.0))

NemoP2AT.uc

class NemoP2AT extends SkidSteeredRobot config(USARBot);



• New classes and data types. Libraries

1. Introduction


3. Integration

4. Experiments

5. Conclusions

geom3d

Vector3D Trans3DPlane

CgalGeometry 3dmapping

PlanarSurface Segmentator3D

FeatureFinder Kalman_Loc

basics

OpenGL

newmat11



• New classes and data types. Libraries

1. Introduction


3. Integration

4. Experiments

5. Conclusions

rdkcoreext

r3dtypes

.

.

.

sensordata

rsensordatalaserdata3d

rlaser3dscan

rlaser3dscanvector

rlaserdata3d

rpointcloud3d



• 3D data acquisition

1. Introduction


3. Integration

4. Experiments

5. Conclusions

capturermodule lasertiltingmodule usarsimclientmodule

laser3dmodule p3atmodule = robot

repository

finaltilt finaltilt desiredtilt desiredtilt

laserdata3dlaserdata3d

laserdata3dlaserdata3d

laserdata3d

finaltilt



• 3D data acquisition

1. Introduction


3. Integration

4. Experiments

5. Conclusions

capturermodule segmentatormodule fittingmodule

repository

pointcloud pointcloud regions

featuresregions

3dmappermodule

features 3dmap

mapperconverter

3dmap

gridmap

Paloma de la Puente . DISAM-UPM 25

2.5. Experiments

• Real environments

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.5. Experiments

1. Introduction


3. Integration

4. Experiments

5. Conclusions


2.5. Experiments

1. Introduction


3. Integration

4. Experiments

5. Conclusions

• USARSim simulated environments


2.5. Experiments

1. Introduction


3. Integration

4. Experiments

5. Conclusions


3. Conclusions

•Feature extraction in three-dimensional environments.

•3D data acquired by tilting a SICK LMS200 laser scanner. Client-server architecture. Communication with the laser at 500kbps.

•Segmentation algorithm based upon the integration of computer vision techniques after computing the residuals of local least squares plane fitting.

•Taking advantage of the ordered nature of the data, real-time capabilities are achieved (~ 1s, 70 x 181 points, cluttered environments).

•Reinforced robustness when under-segmentation occurs. Still very fast.

•Separation of points corresponding to different, not necessarily planar, surfaces.

•Convex-hull based representation of the extracted planar patches.

•6D localization and mapping by means of a Maximum Probability EKF based algorithm.

•Experiments with both real and simulated data.

•RDK modules to gather the 3D data. Processing libraries under construction.

1. Introduction


3. Integration

4. Experiments

5. Conclusions


3. Future work

•Incorporation of unclassified points to represent cluttered areas and unclear objects

•Implementation of the full system within the RDK framework

•Integration of the 3D information into a 2D occupancy grid map to be actually used for navigation in the RoboCup Virtual Robots 2009 competition

•Interpretability enhancement to be accomplished by adding textures to the maps

•Search of more compact models to represent rooms, corridors etc. so that higher level reasoning can be applied to provide topologic and semantic information. Hypothesis and shape restrictions. This will improve the quality and interpretability of the maps.

1. Introduction


3. Integration

4. Experiments

5. Conclusions

INDUSTRIALESETSII | UPM

Paloma de la Puente – [email protected]://intelligentcontrol.es/palomaOpenRDK WorkshopRome, March 16th-19th 2009

DISAM-UPM

3d mapping and openrdk

Documents

d scans

d data acquisition3d

segmentation segmentation

d point cloud

d points cartesian coordinates

segmentation problem

points neighborhood

atpw70serial port