The OpenRDK framework: a not-so-short tutorial Part 1: basic topics
3D Mapping and OpenRDK
description
Transcript of 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
Paloma de la Puente. DISAM-UPM 3
1. Introduction
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 4
2.1. 3D Data Acquisition
• 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
Paloma de la Puente. DISAM-UPM 5
2.1. 3D Data Acquisition
•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
2. 3D Feature Based Mapping2.1. 3D Data Acquisition
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 6
2.1. 3D Data Acquisition
•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
2. 3D Feature Based Mapping2.1. 3D Data Acquisition
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 7
2.1. 3D Data Acquisition
1. Introduction
2. 3D Feature Based Mapping2.1. 3D Data Acquisition
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 8
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
Paloma de la Puente. DISAM-UPM 9
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
2. 3D Feature Based Mapping2.2. Segmentation
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 10
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
2. 3D Feature Based Mapping2.2. Segmentation
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 11
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
2. 3D Feature Based Mapping2.2. Segmentation
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 12
2.2. Segmentation
• Subdivision of regions
- Enhanced robustness
- Still very fast1. Introduction
2. 3D Feature Based Mapping2.2. Segmentation
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 13
2.3. Geometric Model
• 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
Paloma de la Puente. DISAM-UPM 14
2.3. Geometric Model
• 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
2. 3D Feature Based Mapping2.3. Geometrical Model
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 15
2.3. Geometric Model
• Convex-hull based representation to characterize the plane patches
•Region filtering
CGAL libraries
1. Introduction
2. 3D Feature Based Mapping2.3. Geometrical Model
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 16
2.4. Localization and Mapping
• 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
Paloma de la Puente. DISAM-UPM 17
2.4. Localization and Mapping
• 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
2. 3D Feature Based Mapping2.4. Localization and Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 18
2.4. Localization and Mapping
• 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
2. 3D Feature Based Mapping2.4. Localization and Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 19
2.4. Localization and Mapping
• 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
2. 3D Feature Based Mapping2.4. Localization and Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente. DISAM-UPM 20
2.4. Integration within the OpenRDK framework
• Nemo’s USARSim Model
1. Introduction
2. 3D Feature Based Mapping
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);
Paloma de la Puente. DISAM-UPM 21
2.4. Integration within the OpenRDK framework
• New classes and data types. Libraries
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
geom3d
Vector3D Trans3DPlane
CgalGeometry 3dmapping
PlanarSurface Segmentator3D
FeatureFinder Kalman_Loc
basics
OpenGL
newmat11
Paloma de la Puente. DISAM-UPM 22
2.4. Integration within the OpenRDK framework
• New classes and data types. Libraries
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
rdkcoreext
r3dtypes
.
.
.
sensordata
rsensordatalaserdata3d
rlaser3dscan
rlaser3dscanvector
rlaserdata3d
rpointcloud3d
Paloma de la Puente. DISAM-UPM 23
2.4. Integration within the OpenRDK framework
• 3D data acquisition
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
capturermodule lasertiltingmodule usarsimclientmodule
laser3dmodule p3atmodule = robot
repository
finaltilt finaltilt desiredtilt desiredtilt
laserdata3dlaserdata3d
laserdata3dlaserdata3d
laserdata3d
finaltilt
Paloma de la Puente. DISAM-UPM 24
2.4. Integration within the OpenRDK framework
• 3D data acquisition
1. Introduction
2. 3D Feature Based Mapping
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
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 26
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 27
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 28
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 29
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 30
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
• USARSim simulated environments
Paloma de la Puente . DISAM-UPM 31
2.5. Experiments
1. Introduction
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 32
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
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
Paloma de la Puente . DISAM-UPM 34
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
2. 3D Feature Based Mapping
3. Integration
4. Experiments
5. Conclusions
INDUSTRIALESETSII | UPM
Paloma de la Puente – [email protected]://intelligentcontrol.es/palomaOpenRDK WorkshopRome, March 16th-19th 2009
DISAM-UPM