3D SLAM for Omni-directional Camera
Yuttana Suttasupa
Advisor: Asst.Prof. Attawith Sudsang
Introduction Localization
Robot can estimate its location with respect to landmarks in an environment
Mapping Robot can reconstruct the position of landmarks that its
encounter in an environment
SLAM Robot build up a map and localize itself simultaneously
while traversing in an unknown environment
2
The Problem Propose SLAM method for a hand-held omni-
directional Camera Omni-directional camera move freely in an unknown
indoor environment without knowing of camera motion model
Using only bearing data from omni-images and no need any initialize information
Reconstruct 3D camera path and 3D environment map (landmark-based)
3
The Problem Input
a captured image sequence from an omni-directional camera
4
5
The Problem Output
a camera state - 3D position and direction an environment map - 3D landmark positions
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
6
Omni-directional Camera Our omni-directional camera
Two parabolic mirrors CCD camera with 640×480 pixel @ 29.97 Hz 360° horizontal field of view -5° to 65° vertical field of view
7
Omni-directional Camera Normal camera
Omni-directional camera
8
Omni camera Calibration Find a mapping function from 2D image to 3D
object Using Omnidirectional Camera Calibration Toolbox
(Scaramuzza et al., 2006)
9
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
10
EKF SLAM Using extended kalman filter to solve SLAM
problem Assume a robot position and a map probability
distributions as gaussian distributions
Predict a robot position and landmarks distributions using a robot motion model
Correct the distributions using an observation model
11
EKF SLAM The distribution representation
Initial state Assume a robot position distribution with some value at the initial
state
12
Tnkk mmcx 0state
covariance
mmmc
cmcck PP
PPP
robot
probability distribution
EKF SLAM Predict state
Using a robot motion model to predict a robot position
13
kkkk Wuxfx ),( 1
kTkkkk QFPFP
1
Predicted state
Predicted estimate covariance
robot
EKF SLAM Correction state
Using an observation model to update a robot position and landmark positions
14
),ˆ(,, ikkiki mxhzy
kkkk PHKIP )(
Observation model
Updated estimate covariance
kikki vmxhz ),(,
kkkk yKxx ˆˆ
Innovation residual
Updated state estimate
robot landmarkmeasurement
kTkkkk RHPHS
1 kTkkk SHPK
Innovation covariance
Optimal Kalman gain
adjustment
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
15
Introduction to Problem Feature detection Problem
How a computer recognizes objects from an image
Feature association Problem How can we find feature
relations between two images
16
Introduction to Problem Observability Problem
A camera given only a bearing-only data How can we estimate a high dimensional state with low
dimensional measurements
17
Landmark
Camera
How far is it?
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
18
Solution to Problem The proposed algorithm includes 3 steps
Image Processing Detect features Find feature associations Calculate feature measurements
SLAM Apply measurement data to SLAM
Features and reference frames management Add and remove features from SLAM state Add and remove reference frames from SLAM state
19
Solution to Problem System Coordinate
World Frame Camera Frame Reference Frames
20
landmark
World Frame
Camera Frame
Reference Frame
Cy Wy
1Ry 2Ry
Solution to Problem SLAM State
Camera state – represent camera frame Reference frame states – represent reference frames Landmark states – represent landmark positions
21
)ˆ( cx)ˆ( rx
)ˆ( yx
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
22
Image Processing Input
an image from an omni-directional camera
Old SLAM state
Output Feature measurement Feature association
23
Image Processing Feature detection (for new features)
Using point features Finds corners in the image using harris corner detector
24
Image Processing Feature associations
Describe which landmark that the feature in current image is associated to
Find the relation between a current image and old features in an old image
Using optical flow to track features
Using template matching to refine a feature position
25
Image Processing Feature associations – features tracking
Tracking features from a previous image to get a current features position
Using pyramid Lucas-Kanade optical flow
26
Image Processing Feature associations – feature positions refinement
Track features using optical flow may cause a feature drift
Using pyramid template matching to correct a feature position
27current image with a drifted feature
patch
feature
search region
result after refinement using template matching
Image Processing Feature associations – feature position refinements
Select patch from a reference image Patch rotation and scale may not match Transform function may need to apply to patch
28
Reference image
Current image
Not match
Match
Image Processing Feature associations – feature position refinements
Find transform function by project 3D patch creating from a current image to a reference image
29Current image Reference image
Image sphere
3D patch
Image Processing Find transform function
Project every patch pixel may lead to a computational cost problem
Use perspective transform as a transform function instead
Need 4 project points to calculate a perspective transform function
30Real distortion Perspective distortion
Image Processing Feature associations – example
31
Image Processing Feature measurements
Using feature points in omni-image to be a measurement data
Feature points must be converted into bearing-only measurement in the form of yaw and pitch angles
32
)/arctan(
)/arctan( 22
xy
yxz
rr
rrr
z
y
x
landmark
)(
)(
Ray (r)
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
33
Using EKF SLAM to estimate Camera state, Reference frame states and Landmark states
Prediction Determine how a camera move Find state transition model (camera motion model)
Correction How to measurement a landmark Find observation model
34
Simultaneous localization and mapping (SLAM)
Simultaneous localization and mapping (SLAM)
Input Measurement data from
omni-image
Output Estimated SLAM state
Camera state Reference frame states Landmark states
35
Prediction Determine how a camera move But the camera motion is unpredictable Assume that a camera can move freely in any direction
with some limit velocity
36
Simultaneous localization and mapping (SLAM)
Before predict After predict
Correction Using a bunch of measurement (include current
measurements data and old measurements data at reference frame) to update SLAM state
37
Simultaneous localization and mapping (SLAM)
landmark
Current cameraReference frame
Reference frame
Correction
38
Simultaneous localization and mapping (SLAM)
Tui
ui
Cii zzz )2()1(
)/arctan(
)/arctan(
xy
yyxxzXi yy
yyyyyz
Measurement data for landmark i
Observation model for each measurement
y'
landmark
is a landmark position in X coordinatey
Correction step can separate in 2 parts Camera and reference frames Correction Landmarks Correction
39
Simultaneous localization and mapping (SLAM)
Camera and reference frames Correction Assume that the measurement data can measurement
landmark positions accurately The correction affects only a camera state and
reference frame states
40
Simultaneous localization and mapping (SLAM)
Before correction After correction
Landmarks Correction Assume that the camera state is accurate The correction affects only landmark states
41
Simultaneous localization and mapping (SLAM)
Before correction After correction
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
42
Features and reference frames management
Remove features Feature points is out of image bound The landmark position is not accurate enough
43
the feature of this landmark is out of bound
Features and reference frames management
Add features Add new features
using harris corner detector to detect a new feature Add new features when we have a new reference frame
Add old features Consider that the old landmark may be appear in the omni
image again
44
Features and reference frames management
Add new features Add new landmarks to SLAM state Estimate a landmark position by assume a large
variance for a range data
45
Features and reference frames management
Add old features project an old landmark to
the current image
check if a feature available in the image using template matching
46
landmark
Image sphere
feature
Features and reference frames management
Add reference frame When no suitable reference frames for feature tracking When landmark number is below some threshold
Select a current camera state as a new reference frame
47
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
48
Experimental Results
49
Experimental Results
50
Experimental Results
51
Experimental Results
52
Experimental Results
53
Experimental Results
54
Outline Introduction Omni-directional Camera EKF-SLAM Introduction to Problem Solution to Problem
Image Processing SLAM Features and reference frames management
Experimental Results Result Evaluation
55
Result Evaluation Localization Evaluation
2D localization evaluation 3D localization evaluation
Mapping Evaluation
56
Result Evaluation 2D Localization Evaluation
Using wiimote as a bird eye view camera Detect IR point on the omni camera while traversing in
2D plane by a mobile robot
57
IR point
Result Evaluation 2D Localization Evaluation
58
Result Evaluation 2D Localization Evaluation
59
Result Evaluation 2D Localization Evaluation
60
1 12 23 34 45 56 67 78 89 1001111221331441551661771881992102212322432542652762872983093203313420
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
error
average
Result Evaluation 3D Localization Evaluation
Using wiimote attach with an omni-directional camera to localize the 3D camera position related to reference IR board
61
Result Evaluation 3D Localization Evaluation
62
Result Evaluation 3D Localization Evaluation
63
Result Evaluation 3D Localization Evaluation
64
1 15 29 43 57 71 85 99 1131271411551691831972112252392532672812953093233373513653793934074214354494630
0.05
0.1
0.15
0.2
0.25
0.3
0.35
error
average
Result Evaluation Mapping Evaluation
Compare the mapping result with known structure environment
65
Result Evaluation Mapping Evaluation
66
Conclusion Summary
Our algorithm can localize camera position and build up a map in 3D using only a omni-camera image
Omni-directional camera move freely in an unknown indoor environment without knowing of camera motion model
Evaluation Result shows the correspondence of the localization and mapping outcome with the ground truth
67
Thank you
StatisticLocation Conf. room Corridor Stairway
Max feature count 38 36 28
Min feature count 14 13 10
Avg. Feature count 23.9279 20.4866 13.7657
Max landmark count 43 65 43
Min landmark count 25 14 10
Avg. Landmark count 33.1654 45.0207 19.7684
Max time per frame (ms) 1019.73 1424.87 703.188
Min time per frame (ms) 34.9818 19.657 15.1698
Avg. time per frame (ms) 160.925 479.858 172.845
Frame count 804 1157 747
69
Visual SLAM for 3D Large-Scale Seabed Acquisition Employing Underwater Vehicles
70
Featureless Vehicle-Based Visual SLAM with a Consumer Camera
71
Scan-SLAM: Combining EKF-SLAM and ScanCorrelation
72
The Problem Input
a captured image sequence from an omni-directional camera
Output a camera state
3D position and direction an environment map
3D landmark positions
73
Omni camera Calibration
74
Omni camera Calibration
75
)(fv
u
z
y
x
P 22 vu
44
33
2210)( aaaaaf
yc
xc
v
u
e
dc
v
u
1
ycv
xcu
ce
d
v
u
det/det/
det/det/1dec det
Image Processing Feature detection (for new feature)
Using point features Finds corners in the image using harris corner detector
76
Prediction Determine how a camera move But the camera motion is unpredictable Assume that a camera can move freely in any direction
with some limit velocity
77
Simultaneous localization and mapping (SLAM)
TWCk
WCkkckc qtxfx 111,, )ˆ(ˆ
kkckc QPP
1,,
Predicted State
Predicted estimate covariance
Correction
78
Simultaneous localization and mapping (SLAM)
Tui
ui
Cii zzz )2()1(
Tiuiiuiiciirci yrhyrhyxhyxxh )ˆ,ˆ()ˆ,ˆ()ˆ,ˆ()ˆ,ˆ,ˆ( )2()1(
)/arctan(
)/arctan(),(
xy
yyxxzi yy
yyyyyyxh
)(yTyyyy xT
zyx
yxMyTx )()( 1
while is transform function which transform a landmark position (y) from world coordinate to reference coordinate (x)
Measurement data for landmark i
Observation model for landmark i
)(yTx
y'
)(
)(
Camera and reference frames Correction Assume that the measurement data can measurement
landmark positions accurately The correction affects only a camera state and
reference frame states
79
Simultaneous localization and mapping (SLAM)
)ˆ,ˆ,ˆ(ˆ irciii yxxhv RHPHHPHS T
yyTcc
1 SHPK Tc
vKxx ˆˆˆ
PKHIP c )(
Innovation or measurement residual
Innovation (or residual) covariance
Optimal Kalman gain
Update state estimate
Update estimate covariance
is Jacobian Matrix of function h at
is Jacobian Matrix of function h atcH
yHcx̂
yx̂
Landmarks Correction Assume that the camera state is accurate The correction affects only landmark states
80
Simultaneous localization and mapping (SLAM)
)ˆ,ˆ,ˆ(ˆ irciii yxxhv RHPHHPHS T
yyTcc
1 SHPK Ty
vKxx ˆˆˆ
PKHIP y )(
Innovation or measurement residual
Innovation (or residual) covariance
Optimal Kalman gain
Update state estimate
Update estimate covariance
is Jacobian Matrix of function h at
is Jacobian Matrix of function h atcH
yHcx̂
yx̂
Solution to Problem SLAM State
Camera state – represent camera frame Reference frame states – represent reference frames Landmark states – represent landmark positions
81
y
r
c
x
x
x
x
ˆ
ˆ
ˆ
ˆ
yyrycy
yrrrcr
ycrccc
xxxxxx
xxxxxx
xxxxxx
PPP
PPP
PPP
P
TWCWCc qtx ˆ
Tr rrx 21 ˆˆˆ TWRi
WRii qtr ˆ
Ty yyx 21 ˆˆˆ
)ˆ( cx)ˆ( rx
)ˆ( yx
Features and reference frames management
Select reference frame for feature tracking
82
xy
z
x
y
z
cyry
camera frame
reference frame
)cos( rc yy
25.0 r
c
y
y
Features and reference frames management
Select reference frame for update SLAM state
83
xy
z
x
y
z
cyry
camera frame
reference frame
0 rc yy
84
Feature ray
pitch
yaw
landmark
x
y
z
camera)(
)(