The field of 3-D-environment reconstruction has been subject to various re- ..... in addition to the control when sampling a new pose. ...... Orleans, USA, pp.
MEE10:89
Evaluation of Methods for 3D Environment Reconstruction with Respect to Navigation and Manipulation Tasks for Mobile Robots Syed Farzad Husain http://bth.itslearning.com/farzad
This thesis is presented as part of Degree of Master of Science in Electrical Engineering
Blekinge Institute of Technology November 2010 Blekinge Institute of Technology School of Engineering Department of Applied Signal Processing Internal Supervisor: Dr. Siamak Khatibi1 External Supervisor: Dipl.–Ing. Georg Arbeiter2 Examiner: Dr. Siamak Khatibi1
1 2
Blekinge Institute of Technology, Multisensor Group, Karlskrona, Sweden Fraunhofer Institute for Manufacturing Engineering and Automation IPA, Stuttgart, Germany
ii
Abstract The eld of 3-D-environment reconstruction has been subject to various research activities in recent years. The applications for mobile robots are manifold. First, for navigation tasks (especially SLAM), the perception of 3-D-obstacles has many advantages over navigation in 2-D-maps, as it is commonly done. Objects that are located hanging above the ground can be recognized and furthermore, the robots gain a lot more information about its operation area what makes localization easier. Second, in the eld of tele-operation of robots, a visualization of the environment in three dimensions helps the tele-operator performing tasks. Therefore, a consistent, dynamically updated environment model is crucial. Third, for mobile manipulation in a dynamic environment, an on-line obstacle detection and collision avoidance can be realized, if the environment is known. In recent research activities, various approaches to 3-D-environment reconstruction have evolved. Two of the most promising methods are FastSLAM and 6-D-SLAM. Both are capable of building dense 3D environment maps on-line. The rst one uses a Particle Filter applied on extracted features in combination with a robot system model and a measurement model to reconstruct a map. The second one works on 3-D point cloud data and reconstructs an environment using the ICP algorithm. Both of these methods are implemented in GNU C++. Firstly, FastSLAM is implemented.
The object-oriented programming technique is used to build
up the Particle and Extended Kalman Filters. Secondly, 6-D SLAM is implemented. The concept of inheritance in C++ is used to make the implementation of ICP algorithm as much generic as possible. To test our implementation a mo-
R 3 bile robot called Care-O-bot
1 is used. The mobile robot is equipped with
a color and a time-of- ight camera. Data sets are taken as the robot moves in dierent environments and our implementation of FastSLAM and 6-D SLAM is used to reconstruct the maps.
Keywords: SLAM, FastSLAM, 6-D SLAM
1 http://www.care-o-bot.org
iv
Acknowledgments I would like to thank my boss and supervisor Georg Arbeiter at the Fraunhofer Institute from whom nothing escapes.
I am grateful to my internal su-
pervisor Siamak Khatibi at Blekinge Institute of Technology for his help and guidance. I am also indebted to the bright ideas and sharp eyes of the following people: Jan Fischer, Özgür Sen and Daniel Joseph. Thanks are due to all my friends especially Farooq Kifayat, Surkhru Osman, Shahruk Osman and Felix Messmar. Finally, I want to express my hearty gratitude to my family for their continuous encouragement, support and for their love.
v
vi
Contents
Abstract
iii
Acknowledgements
v
List of Algorithms
ix
List of Figures
xii
List of Acronyms 1
2
Introduction
1
1.1
Overview
1.2
Thesis Scope
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
1.3
Thesis Outlines . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
FastSLAM 2.1
2.2 2.3
2.4
3
xiii
3
Introduction to FastSLAM . . . . . . . . . . . . . . . . . . . . . .
3
2.1.1
Background and Related Work
. . . . . . . . . . . . . . .
3
2.1.2
Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . .
3
2.1.3
Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . .
3
2.1.4
FastSLAM
. . . . . . . . . . . . . . . . . . . . . . . . . .
Characteristics of FastSLAM Implementation Details
. . . . . . . . . . . . . . . . . . . .
4 4
. . . . . . . . . . . . . . . . . . . . . . .
5
2.3.1
Why FastSLAM 1.0 and not FastSLAM 2.0 . . . . . . . .
5
2.3.2
Procedure for FastSLAM 1.0
. . . . . . . . . . . . . . . .
5
Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . .
8
6-D SLAM 3.1
1
15
Introduction to 6-D SLAM
. . . . . . . . . . . . . . . . . . . . .
3.1.1
Background and Related Work
3.1.2
ICP Algorithm
. . . . . . . . . . . . . . .
15 16
. . . . . . . . . . . . . . . . . . . . . . . .
16
3.2
Characteristics of ICP Algorithm . . . . . . . . . . . . . . . . . .
17
3.3
Our Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
3.4
ICP with Color Images . . . . . . . . . . . . . . . . . . . . . . . .
18
3.4.1
18
Implementation Details 3.4.1.1
3.4.2 3.5
. . . . . . . . . . . . . . . . . . . .
19
Experimental Results
. . . . . . . . . . . . . . . . . . . .
20
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
Frustum ICP 3.5.1
. . . . . . . . . . . . . . . . . . .
Registration
Implementation Details vii
. . . . . . . . . . . . . . . . . . .
27
3.5.2 3.6
4
Experimental Results
. . . . . . . . . . . . . . . . . . . .
29
Estimating Rigid Body Transformation . . . . . . . . . . . . . . .
29
3.6.1
Singular Value decomposition of a Matrix . . . . . . . . .
30
3.6.2
Computing Transformation Using Unit Quaternion . . . .
30
3.6.3
Computing Transformation by Approximation
31
. . . . . .
Discussion
33
4.1
FastSLAM Versus 6-D SLAM . . . . . . . . . . . . . . . . . . . .
33
4.2
Conclusion and Future Work
36
. . . . . . . . . . . . . . . . . . . .
Bibliography
37
Appendices
39
A List of External Libraries
41
A.1
ANN: A Library for Approximate Nearest Neighbor Searching . .
A.2
Bayesian Filtering Library . . . . . . . . . . . . . . . . . . . . . .
42
A.2.1
Gaussian Distribution
. . . . . . . . . . . . . . . . . . . .
42
A.2.2
Resampling Function . . . . . . . . . . . . . . . . . . . . .
42
viii
41
List of Algorithms 3.1
ICP Algorithm
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
3.2
Proposed ICP . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
ix
x
List of Figures 1.1
Care-O-bot 3
2.1
Simulated robot poses in world coordinates
. . . . . . . . . . . .
6
2.2
Simulated landmark Poses in world coordinates . . . . . . . . . .
8
2.3
Sequence of images of a kitchen environment
8
2.4
Front view of the reconstructed kitchen environment built using FastSLAM
2.5
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
9
Top view of the reconstructed kitchen environment built using FastSLAM
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.6
Sequence of images of a table
. . . . . . . . . . . . . . . . . . . .
10
2.7
Front view of the reconstructed table built using FastSLAM . . .
10
2.8
Top view of the reconstructed table built using FastSLAM
. . .
11
2.9
Top view of the table without the application of FastSLAM . . .
12
2.10 [Left] Original path - [Right] Noise added path (gray) & Corrected path(red)
θz ,
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Pitch
θy
θx
3.1
Yaw
3.2
Target (red) and Source (blue) point clouds . . . . . . . . . . . .
20
3.3
Color images along with the corresponding points . . . . . . . . .
21
3.4
Resultant point cloud after applying ICP algorithm . . . . . . . .
21
3.5
Front view of the kitchen map built using ICP based SLAM . . .
22
3.6
Top view of the kitchen map built using ICP based SLAM . . . .
22
3.7
RMS Error for dierent sets of point clouds with known initial alignment
3.8 3.9
and Roll
rotations . . . . . . . . . . . . . . .
13 15
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
Convergence using standard ICP, target (red), source (blue) . . .
24
Convergence using standard ICP on the subset of point clouds only, target (red), source (blue) . . . . . . . . . . . . . . . . . . .
24
3.10 Resulting point clouds after transformation, target (red), source (blue)
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
3.11 RMS Error for dierent sets of point clouds with unknown initial alignment
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.12 Sequence of images in an indoor environment 3.13 Front view of the indoor environment 3.14 Top view of the indoor environment
25
. . . . . . . . . . .
26
. . . . . . . . . . . . . . .
26
. . . . . . . . . . . . . . . .
27
. . . . . . . . . . . . . . . . . . . . . . . . . .
28
3.16 Target (red) and Source (blue) point clouds . . . . . . . . . . . .
29
3.15 Visibility Frustum
4.1
Maps built with FastSLAM (left) and 6-D-SLAM (right) . . . . .
33
4.2
Reconstruction of table using Frustum ICP
34
xi
. . . . . . . . . . . .
4.3
Reconstructed planes of kitchen environment
4.4
Reconstructed surface of kitchen map built using FastSLAM (left)
. . . . . . . . . . .
and 6-D SLAM (right) . . . . . . . . . . . . . . . . . . . . . . . .
xii
35 35
List of Acronyms SLAM: Simultaneous Localization and Mapping RBPF: Rao Blackwellised Particle Filter SURF: Speeded Up Robust Features ICP: Iterative Closest Points SVD: Singular Value Decomposition UQ: Unit Quaternion
xiii
xiv
Chapter 1 Introduction
1.1
Overview
In the eld of robotics, when a fully automated task is to be performed then one of the most essential requirement is to have a map of the surrounding environment and the relative position of the robot. The more detailed this information is acquired, the more it becomes possible to perform complicated tasks. For example, if a robot is capable of moving and operating in all the three dimensions of space but the map is known only in two dimensions then it will restrict the capability of the robot. Also the level of complexity and probability of error in performing a task depends on how much densely and accurately the structure of the environment is known. When tasks are to be performed with full automation then both the robot pose and the map of the surrounding environment are unknown and should be determined simultaneously.
This problem is commonly referred to as the
Simultaneous Localization and Mapping or SLAM problem. Various methods have been proposed to solve the SLAM problem which are mostly based on a seminal paper by Smith and Cheeseman [1]. Two of the methods which are being used most commonly to solve this problem are FastSLAM [2] and 6-D-SLAM [3]. There are two versions of FastSLAM. FastSLAM 1.0 [2] and FastSLAM 2.0 [27]. The dierence is that the later incorporates the most recent measurement in addition to the control when sampling a new pose.
ICP (Iterative Closest Points ) [4] algorithm for registration of raw 3-D point cloud data.
The second method uses the the
1.2
Thesis Scope
The aim of this work is to implement the FastSLAM and 6-D-SLAM methods, and test them under dierent real world scenarios of indoor environments. Then make a comparison between these two methods and enlist the pros and cons of
R 3. their usage. The implementation was tested on a robot called Care-O-bot Figure 1.1 shows the image of the robot. 1
Figure 1.1: Care-O-bot 3
1.3
Thesis Outlines
This thesis report is organized into four chapters. Chapter 1 gives a general introduction to the thesis topic and outlines the scope of the work. Chapter 2 is organized into four sections. Section 2.1 introduces the FastSLAM algorithm and explains its underlying details.
Section 2.2 enlists the
characteristics of FastSLAM algorithm. Section 2.3 explains the in depth implementation details of FastSLAM. Section 2.4 shows the experimental results of the application of FastSLAM in dierent real world scenarios. Chapter 3 is organized into six sections.
Section 3.1 introduces the 6-D-
SLAM problem and explains the underlying ICP algorithm. Section 3.2 enlists the characteristics of ICP algorithm. Section 3.3 tells about the approach we used for implementing ICP algorithm.
Section 3.4 gives the implementation
details and shows the experimental results of the ICP algorithm when using the color images. Section 3.5 gives the implementation details and shows the experimental results of using Frustum-ICP approach. Section 3.6 gives a detailed explanation of the dierent rigid body transformation estimation methods used in the ICP algorithm. Finally in Chapter 4, dierent aspects of FastSLAM and 6-D-SLAM are compared in section 4.1 and the thesis is concluded along with the outline for future work in section 4.2.
2
Chapter 2 FastSLAM 2.1
Introduction to FastSLAM
FastSLAM is an algorithm which is primarily based on Kalman and Particle Filtering.
2.1.1
Background and Related Work
FastSLAM 1.0 was introduced by Montemerlo
et al. [2] which is capable of solv-
ing the SLAM problem in a robust way. Compared to EKF SLAM [1] approach which has a computational complexity of
O N2
where
N
is the number of
landmarks, FastSLAM reduces the computational complexity to where
M
O (M log (N ))
is the number of particles.
Later the same authors extended it to FastSLAM 2.0 [27], which is an improved version of FastSLAM 1.0. As explained in [28], in case of low error in measurement FastSLAM 1.0 starts to diverge and in order to avoid divergence the number of particles need to be increased resulting in increased computational time but FastSLAM 2.0 does not diverge in this case. Our implementation closely follows [29], where they use SURF features as landmarks but they use stereo vision for computing depth information, where as we use a time-of- ight sensor for the same purpose.
2.1.2
Kalman Filter
Kalman Filter is an algorithm which is used to estimate the optimal value of current state
xt
using the old estimate
xt−1 and
current observation
zt .
The
Kalman ltering algorithm is limited to linear systems which can be extended to non-linear systems. This can be done by linearizing the non-linear system and measurement model. This method is known as Extended Kalman Filtering or EKF.
2.1.3
Particle Filter
A Particle Filter is used to estimate a current set of states
n o [1] [2] [k] xt , xt , . . . xt
by nding a set of weighted samples from a previous set of states and a given current observation
zt and
control
ut . 3
the previous state
[k]
x ˆt [k] xt−1
A state sample
is rst generated given the probability distribution of
ut . i.e. [k] ∼ p xt |xt−1 , ut
and control
1. Draw a sample from
[k]
x ˆt
2. Then the estimated state is weighted based on the given observation. i.e.
[k] [k] wt = p zt |ˆ xt Finally the estimated states are resampled on the principle that the states having higher weights are sampled more often than those having lower weights. Compared to a Kalman Filtering, the Particle Filtering can be used with non-linear Bayesian states.
Since it estimates a distribution by sampling, so
in the limit, as the number of particles approaches in nity the approximation approaches exactness. Since the introduction of Particle Filters in 1993 [5], this ltering method has been evolving with the passage of time. A detailed and up-to-date tutorial on Particle Filtering can be found in [6].
2.1.4
FastSLAM
The problem with Particle Filtering is that with the increase in the number of dimensions the size of the state space over which we need to sample increases drastically, resulting in the requirement of number of particles to increase exponentially. So Particle Filters are usually restricted to problems having lower dimensions. FastSLAM solves this by factoring the SLAM problem into an independent landmark estimation problem, given an estimate of the robot path. This technique of analytic marginalization of landmark estimation using Extended Kalman Filters conditioned on robot path estimate is an instance of
Rao-Blackwellisation [30]. In this way the number of dimensions is reduced to the dimensions of the robot pose.
2.2
Characteristics of FastSLAM
The following are the characteristics of the FastSLAM algorithm. 1. This algorithm works only for static environments. 2. It uses a Particle Filter, where each particle represents a set of probability distributions. 3. The pose of landmarks is represented by Extended Kalman Filters with a mean and a covariance. 4. Every particle has its own set of landmarks and robot pose. 5. Every particle has a weight.
The particle having the maximum weight
represents the most probable set of probability distributions for robot and landmark pose i.e. the most accurate representation of the environment and robot pose.
4
2.3
Implementation Details
There are dierent variants of FastSLAM which are adapted according to the measurement and system model. The variant that we use is FastSLAM 1.0 with known correspondences and multiple observations per time step. We are having multiple observations per time step because we are using SURF descriptors as a landmark.
SURF (Speeded Up Robust Features) is a
scale invariant and rotation invariant interest point detector and descriptor [7]. So after every time step we get a set of 3D points which correspond to SURF descriptors that are extracted from the color images. We can track these feature points in the following steps based on the descriptor values, so we have known correspondences after every step.
2.3.1
Why FastSLAM 1.0 and not FastSLAM 2.0
FastSLAM 2.0 could also have been used but for multiple observations per time step it becomes more complicated. As explained in [8], in this case we will need to nd a way to process the measurements that will give new landmarks to be processed in the end. This is required because if the motion model is noisy then the initial pose estimate of the new landmark may get quiet inaccurate. In order to determine the new landmarks after every step, we will need to compute the eld of view after every step. Also since we are using a kd-Tree to speed up the search process, it becomes almost impossible to reorder the landmark once the kd-Tree is built.
And in order to reorder the landmarks
before building up a kd-Tree will require an extra loop to go through all the features to check whether the feature points lie inside or outside the eld of view.
2.3.2
Procedure for FastSLAM 1.0
First a set of particles are created. Each particle carries a weight, stores a robot pose and a set of landmarks. In order to speed up the landmark search process in every step, the map in every particle (the set of landmarks) is stored in the form of a kd-Tree. A.1.
Details of kd-Tree implementation are given in appendix
The pose of a landmark is represented by a Gaussian distribution (see
appendix A.2.1). After every time step, Initially the robot pose is updated in every particle based on the control information noise level of the system.
ut
from the odometry and the
This is a simulated guess.
Since we are assuming
Gaussian noise so the value of the robot pose is spread around a mean variance
σ
in the particles.
particles with
µr
µ
and
Figure 2.1 shows simulated robot poses for 100
equal to robot pose from odometry and 5
σr = 0.05m.
Robot Poses −2.7
Distance along y−axis (meters)
−2.75
−2.8
−2.85
−2.9
−2.95
−3
2.7
2.75 2.8 2.85 Distance along x−axis (meters)
2.9
2.95
Figure 2.1: Simulated robot poses in world coordinates
When a landmark is added, it is rst checked using its SURF descriptor if it is new or old.
If it is new then it is added in a global kd-Tree and in the
map of every particle as a new landmark. The mean value of the pose of the landmark is determined by transforming the measurement from robot into world coordinates using the simulated robot pose for every particle. i.e.
µxw
= xrob · cos (µθr ) − yrob · sin (µθr ) + µxr
(2.1)
µyw
= xrob · sin (µθr ) + yrob · cos (µθr ) + µyr
(2.2)
µzw
=
zrob
(2.3)
µxw , µyw , µzw is the mean value of landmark pose in world coordinates xrob , yrob , zrob is the landmark pose (measurement) in robot coordinates and µxr , µyr , µθr is the mean value of the simulated robot pose in world coordinates. where
and
The measurement covariance can be determined by
Σ where
G
=
−1 GT R−1 G
(2.4)
is calculated by taking the Jacobian of the robot pose with respect
to the measurement (landmark pose in world coordinates). matrix that models the measurement noise. 6
R
is the covariance
If the landmark is determined to be an old one by the SURF descriptor then it is passed as a known landmark to every particle. The probability of correct association is computed in every particle using eq. 2.5.
p
where
1 1 T −1 √ exp − (znew − zprv ) Σ (znew − zprv ) 2 2πΣ
=
znew
is the new pose of the landmark and
pected position of the landmark.
zprv
zprv
(2.5)
is the predicted, ex-
can be calculated by transforming the
landmark pose stored in the particle back to the robot coordinates. This transformation can be done using eq. 2.6,2.7,2.8.
xrob
yrob
=
=
(µxw − µxr ) · cos (µθr ) + (µyw − µyr ) · sin (µθr )
(2.6)
− (µxw − µxr ) · sin (µθr ) + (µyw − µyr ) · cos (µθr )
(2.7)
zrob
=
µzw
(2.8)
So if the predicted pose of the landmark and the pose that we get from the measurement are close to one another will give a higher probability, implying the association was correct. Otherwise it will be wrong. If the probability is higher than a certain threshold then EKF update equations will be applied to nd the new pose of the landmark. Since we are using a global map to compute initial correspondence, the features that are rejected by the best particle (the particle having the maximum weight) are added as a new feature to the global map. As explained in [8] for multiple observations per step, the weight of the particle can be calculated by multiplying the individual weights for every landmark. Finally the resampling method (see appendix A.2.2) will be used to remove the particles having lower weights. The particles having higher weights will be sampled more often then the ones having lower weights. Figure 2.2 shows the simulated poses of a landmark for 100 particles in world coordinates in 2D. 7
Landmark Poses 0.4
Distance along y−axis (meters)
0.3
0.2
0.1
0
−0.1
−0.2
−0.3 −1.85
−1.8
−1.75 −1.7 −1.65 −1.6 Distance along x−axis (meters)
−1.55
−1.5
Figure 2.2: Simulated landmark Poses in world coordinates
2.4
Experimental Results
The implementation is tested using a color and a time-of- ight camera, that is
R . Data is taken as the robot moves in dierent mounted on top of Care-O-bot environments.
Figure 2.3: Sequence of images of a kitchen environment
Figure 2.3 shows a series of color images taken in a kitchen environment. The lines show the association of every feature with the last image (bottom 8
right), found using FastSLAM. The numbers tell the associated feature's id.
Figure 2.4:
Front view of the reconstructed kitchen environment built using
FastSLAM
Figure 2.5: Top view of the reconstructed kitchen environment built using FastSLAM Figures 2.4 and 2.5 show the view from front and top of the kitchen map respectively, built using FastSLAM. 9
Figure 2.6: Sequence of images of a table
Figure 2.6 shows a series of color images taken as the robot moved around a table. The lines show the correspondences between the rst image (top left) and the last image (bottom right) as the robot came back to its starting position. Figure 2.7 and 2.8 show the view from front and top of the table map respectively, built using FastSLAM.
Figure 2.7: Front view of the reconstructed table built using FastSLAM
10
Figure 2.8: Top view of the reconstructed table built using FastSLAM
11
In order to test the e ciency of FastSLAM algorithm, the data collected for the table environment (cf. Figure 2.6) was transformed using plain odometry without the application of FastSLAM. Figure 2.9 shows the view from top of the table without applying the FastSLAM algorithm. A comparison of Figure 2.8 with Figure 2.9 shows how much FastSLAM is able to correct the pose of each feature.
Figure 2.9: Top view of the table without the application of FastSLAM
12
Finally Gaussian distributed noise is added to the robot path which we obtain from the odometry and then it is checked up to how much FastSLAM was able to correct it. Figure 2.10 shows robot path obtained from odometry on the left and path with Gaussian added noise with mean
σ = 0.02m
µ=0
and variance
on the right (gray color) and the corrected path (red color).
Figure 2.10: [Left] Original path - [Right] Noise added path (gray) & Corrected path(red)
13
14
Chapter 3 6-D SLAM
3.1
Introduction to 6-D SLAM
6-D SLAM or Simultaneous Localization and Mapping with 6 Degrees of Freedom is a method used to build dense 3-d maps and at the same time determine pose and orientation of the ranging device. This is done by consistent matching of a set of points with another set of points by nding a transformation. This method is called
registration. In order to fully identify the pose and orientation
of the device 6 parameters will be needed. 3 to identify the spatial position i.e.
{tx , ty , tz }
and 3 to identify the spatial orientation i.e.
{θx , θy , θz }.
eters that determine the orientation are commonly referred to as
roll angles [9]. Figure 3.1 show the yaw, pitch and roll rotations.
Figure 3.1: Yaw
θz ,
Pitch
θy
15
and Roll
θx
The param-
yaw, pitch and
rotations
3.1.1
Background and Related Work
6-D SLAM is based on ICP algorithm. ICP or Iterative Closes Points was initially introduced by Besl and McKay [10] and Chen and Medioni [11].
Since
then it has been used extensively for matching of two sets of points. The proposed algorithm describes a method to register point or line segment sets in 3-D space. Although the term 6-D SLAM was rst used by Nütcher [12] in 2004 where he uses a variant of ICP to perform registration of points got from a laser scanner, ICP algorithm has been used for similar tasks before by others as well [13, 14].
3.1.2
ICP Algorithm
Given two sets of point clouds, Source computes the rotation matrix
R
S
and Target
T, t
and translation vector
the ICP algorithm by minimizing the
error function in eq. 3.1.
E(R, t)
=
N X
k (R · pi + t) − qi k2
(3.1)
i=1
i − th point in the Source that corresponds to qi point in the R is the (3 × 3) rotation matrix and t is the (3 × 1) translation vector. The (3 × 3) rotation matrix is found by multiplying all the three individual rotation matrices Rx , Ry , Rz which Where
N
Target.
pi
is the
is the total number of corresponding points.
are used for rotation around a single axis of rotation. Combining this with the
(3 × 1)
translation vector gives us the complete transformation matrix
T =
=
R(3×3) t(3×1) 0 0 0 1
T
cos (θx ) cos (θy ) cos (θx ) sin (θy ) sin (θz ) − sin (θx ) cos (θz ) sin (θx ) cos (θy ) sin (θx ) sin (θy ) sin (θz ) + cos (θx ) cos (θz ) . . . −sin (θy ) cos (θy ) sin (θz ) 0 0
...
i.e.
cos (θx ) sin (θy ) cos (θz ) + sin (θx ) sin (θz ) sin (θx ) sin (θy ) cos (θz ) − cos (θx ) sin (θz ) cos (θy ) cos (θz ) 0
tx ty tz 1
(3.2)
The ICP algorithm is summarized in Algorithm 3.1. 16
Algorithm 3.1 ICP Algorithm 1: for i = 0 to 2:
maxIterations do
Find correspondences
(pi , qi )
between Source
S
and Target
T
point
clouds. 3:
Calculate transformation
(R, t) that minimizes the error function eq.
3.1. Apply the transformation(R, t) to the data set
4: 5:
S.
Compute the dierence of the quadratic error, i.e., compute the dierence of the value
kEi−1 (R, t) − Ei (R, t)k
application of the transformation. threshold
ε,
before and after the
If this dierence falls below a
terminate.
8: end for
3.2
Characteristics of ICP Algorithm
Following are the characteristics of the ICP algorithm. 1. The Target point cloud must be a proper subset of Source point cloud. 2. ICP algorithm might converge in a wrong convergence basin depending on the initial position and orientation of the Source point cloud with respect to the Target point cloud. 3. ICP algorithm is prone to noise. The performance of the algorithm degrades with the increase in noise level.
3.3
Our Approach
We tested two dierent variants of the ICP algorithm. First one uses the color images to nd the overlapping region. Also a set of corresponding points were found using these images, which served to compute the initial transformation. After applying this initial transformation to the Source point cloud, the ICP algorithm with slight modi cation (mentioned in section 3.4.1.1) is applied on the complete point cloud of Target and the overlapping region of the Source. In the second variant we use Frustum ICP to nd the transformation. For this we need the robot pose from odometry to nd the visibility frustum which in turn tells us about the points in the Source point cloud that overlap with the Target. Then only overlapping points are used in the modi ed ICP algorithm. For minimizing eq. 3.1 we need to compute a rigid body transformation in every iteration of ICP algorithm. Three methods were tested which are discussed in section 3.6. Finally point clouds were merged together into one point cloud based on the principle that if the distance between the nearest points between Source and Target lie below a certain threshold then they are considered as one. Then this point cloud is considered as a Target point cloud for next step. 17
3.4
ICP with Color Images
3.4.1
Implementation Details
First a colored 3D point cloud is created i.e. each point has 6 dimensions. 3 for describing its spatial position and 3 for its color. Then the color Image is used to nd the interesting points.
For that purpose SURF (Speeded Up Robust
Features) is used, which is a scale invariant and rotation invariant interest point detector and descriptor [6]. Using the SURF descriptors an initial set of corresponding points is created. Also using the color images the region in the source image that overlaps with the target image is found. This is done by dividing the source image into small segments and searching each segment in the target image. We use the normalized correlation coe cient to compare the dierence. Its implementation from the OpenCV library
1 is used. The formula for nding
normalized correlation coe cient is given in eq.
3.3.
Here
Sseg
is the image
segment from the source image used for searching in the target image
T. R
is
the resulting coe cient.
0 (i0 , j 0 ) × T 0 (i + i0 , j + j 0 ) Sseg qP P 0 0 0 2 0 0 0 2 i0 ,j 0 Sseg (i , j ) × i0 ,j 0 T (i + i , j + j ) P
R(i, j)
=
i0 ,j 0
(3.3)
where
0 (i0 , j 0 ) = Sseg (i0 , j 0 ) − Sseg
1 (w×h)
×
P
T 0 (i + i0 , j + j 0 ) = T (i + i0 , j + j 0 ) −
i00 ,j 00
Sseg (i00 , j 00 )
&
1 (w×h)
×
P
i00 ,j 00
T (i + i00 , j + j 00 )
If the coe cient is greater than a threshold, then the corresponding points in the source point cloud are marked as the overlapping points and only those are used in the registration process.
The rest are marked as non-overlapping
ones. So there are two pairs of point clouds now, which are used in the ICP algorithm. 1. Complete target point cloud and Overlapping Region of source point cloud.
2. Point Cloud of corresponding points generated from SURF descriptors. Our assumption is that we do not have any kind of information from odometry. So we are relying on the SURF algorithm for initial transformation
Minitial ,
so
that the point clouds are aligned in the correct convergence basin. Then we run ICP algorithm with the proposed modi cation described in section 3.4.1.1.
1 http://opencv.willowgarage.com/documentation/index.html 18
3.4.1.1
Registration
Algorithm 3.2 Proposed ICP Given source and target point cloud subset in our case
S, T
and source and target point cloud
SSU RF , TSU RF
1. Compute overlapping region in source:
Ssub
2. Compute initial point correspondence (SURF descriptor in our case) 3. Compute initial registration matrix
Minitial
4. Apply transformation
(Ssub , T )
5. Alternate between
and
(SSU RF , TSU RF )
6. Create closest point pairs 7. Compute registration matrix
M
8. Find RMS point to point distance error 9. If converged then terminate 10. Go to step 4
Our proposed ICP method for registration is summarized in algorithm 3.2. In step 6 we use a point pairs.
k -D
tree with
k = 3
as proposed in [15] for creating closest
This helps in reducing the search time.
dierent converging values. One is convergence using using
(SSU RF , TSU RF ).
In step 8 there are two
(Ssub , T )
and the other
Whichever gives the minimal value of error should be
taken as nal and the algorithm should be terminated then. Also it should be noted that it is not necessary to use these pairs alternatively. It is also possible to use one pair more often than the other depending on various parameters like noise level, overlapping region. As information from color images is highly dependent on texture from the environment. The basic idea behind our proposal is that, if we are able to nd a subset of point cloud from source and target which are more likely to have a correspondence i.e. fewer points in the source set that do not belong to the target then we can use the alternating ICP approach to make ICP converge in fewer iterations and in correct local minimum. This is because ICP will be directed to converge to a minimum that is common to both pairs of point clouds. The new error function that has to be minimized becomes
E(R, t)
=
Nj X
k (R · pi,j + t) − qi,j k2
(3.4)
i=1 Where the index sponding points
p
j
and
tells which point cloud pair is being used for the corre-
q.
Due to the non-overlapping points and noise in the coordinate values it is possible that the global minimum is not the correct minimum or even in worst 19
case scenario the minimum does not even exist that gives the desired alignment. This can happen if we reduce the point cloud size by uniform sampling in order to increase the computational speed. But because of the point cloud of SURF descriptors the correct minimum will stay. In that case ICP will not converge rather it will start to oscillate between the correct minimum and approach towards another wrong minimum. It depends on how often we alternate between the SURF descriptor point cloud and the entire point cloud.
3.4.2
Experimental Results
Figure 3.2: Target (red) and Source (blue) point clouds
Figure 3.2 shows the two point clouds, Target in red color and Source in blue color and the initial corresponding points. The points in the dark blue are the non-overlapping points and the points in light blue are the overlapping points found using the color images in gure 3.3. Figure 3.3 also shows the point correspondences found using the SURF descriptors which are used to compute the initial transformation. Figure 3.4 shows the resultant point cloud after applying proposed ICP algorithm 3.2. 20
Figure 3.3: Color images along with the corresponding points
Figure 3.4: Resultant point cloud after applying ICP algorithm
Figure 3.5 and 3.6 show the view from front and top of the kitchen map respectively, corresponding to the images shown in Figure 2.3. This map was built using ICP with small angle approximation explained in section 3.6.3 for nding transformation in every iteration. 21
Figure 3.5: Front view of the kitchen map built using ICP based SLAM
Figure 3.6: Top view of the kitchen map built using ICP based SLAM In order to compare our modi ed ICP approach with the standard ICP approach we tested both of them with the same point clouds of gure 3.2. Figure 3.7 shows a comparison of the RMS error values. 22
Once the standard
ICP was tested using SURF features only and once it was tested with the raw data. Although the ICP algorithm converged for all the three cases because we aligned the point clouds initially using the point correspondences from SURF features but it can be seen that the modi ed ICP and ICP using SURF features converged in fewer iterations.
3 RMS Error modified ICP RMS Error standard ICP with raw point clouds only RMS Error standard ICP SURF features only
2.5
RMS Error
2
1.5
1
0.5
0
0
Figure 3.7:
10
20
30
40 50 60 no. of iterations
70
80
90
100
RMS Error for dierent sets of point clouds with known initial
alignment
The eect of modi ed ICP is most pronounced when it is tested with the same point clouds shown in gure 3.2 without using the initial point correspondence and without computing the overlapping region.
SVD method is used
for nding transformation and the algorithm converged only when we alternate between the two point clouds. Figure 3.8 shows the result of using standard ICP on raw point clouds without using any initial transformation. Figure 3.9 shows the result of using the subset of point clouds only, again without any initial transformation. Figure 3.10 shows the result of using the alternating approach. Figure 3.11 shows a plot of RMS (Root Mean Square) point to point distance error value, after every iteration, for all the 3 cases mentioned before.
From
gure 3.8, 3.9, 3.10 and 3.11 it can be seen that ICP algorithm converged in the correct convergence basin, only with the alternating approach and it rather started to diverge for the rest. Finally the algorithm was tested inside another indoor environment. Figure 3.12 shows the sequence of images as the robot moved in the environment. Figure 3.13 and 3.14 show the view from front and top of the map respectively. 23
Figure 3.8: Convergence using standard ICP, target (red), source (blue)
Figure 3.9: Convergence using standard ICP on the subset of point clouds only, target (red), source (blue)
24
Figure 3.10: Resulting point clouds after transformation, target (red), source (blue)
16 RMS Error standard ICP corresponding to Figure 2 RMS Error standard ICP corresponding to Figure 3 RMS Error modified ICP corresponding to Figure 4
14 12
RMS Error
10 8 6 4 2 0
0
5
10
15
20 25 no. of iterations
30
35
40
Figure 3.11: RMS Error for dierent sets of point clouds with unknown initial alignment
25
Figure 3.12: Sequence of images in an indoor environment
Figure 3.13: Front view of the indoor environment
26
Figure 3.14: Top view of the indoor environment
3.5
Frustum ICP
The non-overlapping points in the Source image are separated from the overlapping ones using the visibility frustum of the range sensor. This method of removal of points based on visibility frustum is called frustum culling [16] a nd combined with ICP algorithm it is called Frustum ICP [17].
3.5.1
Implementation Details
The two point clouds Target and Source are transformed using the data from odometry. Then points of the Source image that lie outside the visibility frustum of the Target point cloud are not used in ICP algorithm. the frustum, 4 vectors
→ − − → − → − a , b ,→ c, d
and vertical eld of view of the range sensor. the
4
In order to create
are computed based on the horizontal Then based on these vectors
normal vectors of the clipping planes, i.e. up, bottom, left and right are
computed and nally the points that lie inside the four clipping planes are found using a simple dot product. Figure 3.15 shows the visibility frustum of the range imaging sensor. 27
nup
nright
nleft
nbottom Figure 3.15: Visibility Frustum The normal vectors of the clipping planes are computed by
→ − n up
→ − n bottom
If the point lies inside the
→ − → b ×− a
=
=
(3.5)
→ − → d × −c
→ − n lef t
→ − − = → a × d
→ − n right
− −c × → = → b
4
(3.6)
(3.7)
(3.8)
clipping planes, than the dot product
x · nx + y · ny + z · nz
0
(3.10)
for all the normal vectors. If it lies outside then
x · nx + y · ny + z · nz
So if the dot product becomes greater than zero for even one of the clipping planes then it means the point lies outside the frustum. The dot product will always be greater than zero if the point is above the surface because
cos (θ) > 0 cos (θ) < 0
∀
28
−90 < θ < 90 otherwise
(3.11)
3.5.2
Experimental Results
Figure 3.16 shows the Target points in red, the Source points that lie inside the frustum of the Target in light blue and the points that lie outside the frustum in dark blue color.
Figure 3.16: Target (red) and Source (blue) point clouds
Since the points of the Source were transformed already using the information from odometry before applying ICP so the points are already close enough to the Target points. Thus in this case the ICP algorithm does not show much eect, compared to the case when the odometric information was not used (cf. Section 3.4).
3.6
Estimating Rigid Body Transformation
We test two closed form solutions and a small angle approximation for nding transformation in every iteration of ICP algorithm. The closed form solutions are namely computing transformation using Singular Value Decomposition (SVD) and using Unit Quaternion (UQ). A comparison of these methods can be found in [24]. It is shown in [24] that the results of both the methods are similar in most cases. But for our data set it was found that in the absence of initial point correspondences, UQ based minimization converges sometimes in the correct convergence basin when SVD based minimization fails to converge. The third method does not give a closed form solution rather it solves the ICP algorithm by making small angle assumption.
This method proved to be the
most robust and required the least number of iterations. 29
3.6.1
Singular Value decomposition of a Matrix
This method was presented in 1987 by Arun, Huan and Blostein [18]. Two point sets
{pi }
and
{qi }
where
i = 1, 2, . . . N qi
where
R
=
is the rotation matrix,
t
are related by
Rpi + t + vi is the translation vector and
vi
is a noise
vector. Given
{pi }
and
rotation matrix
R
{qi }
it is possible to nd the least square solution of a
t which is based on the singular value {pi } and {qi } are considered as 3 × 1
and translation vector
decomposition of a
3×3
matrix. Here
vectors. It was proved in [18] that if
Γ2
N X
=
kqi0 − Rp0i k2
(3.12)
i=1
p0i , pi − cS ˆ which minimizes Γ2 in eq. 3.12. Here cS and Then we can use SVD to nd R cT are the centroids of the point sets {pi } and {qi } respectively. The translation where
qi0 , qi − cT
,
can be found using
ˆ S tˆ = cT − Rc
(3.13)
Proof can be found in [18, 19].
3.6.2
Computing Transformation Using Unit Quaternion
The transformation can also be calculated using unit quaternion. This method was presented in 1987 by Horn [20]. A quaternion can be thought of as a complex number with three dierent imaginary parts [21]. Summary of the equations given in [22] is reproduced here. First we compute the cross covariance matrix represented as unit quaternion.
(σxx + σyy + σzz ) (σyz + σzy ) (σ + σ ) (σ yz zy xx − σyy − σzz ) Σ= (σzx + σxz ) (σxy + σyx ) (σxy + σyx ) (σyz + σzy )
(σzx + σxz ) (σxy + σyx ) (σxy + σyx ) (σzx + σxz ) (−σxx + σyy − σzz ) (σyz + σzy ) (σzx + σxz ) (−σxx − σyy + σzz ) (3.14)
where
σxx =
N P
0 qx,i p0x,i ,σxy =
i=1
N P
0 qx,i , p0y,i
...
i=1
.
The rotation represented as unit quaternion
Q,
that minimizes eq.
3.1,
corresponds to the largest eigenvalue of the cross covariance matrix in eq. 3.14. Then convert the eigen vector corresponding to the largest eigen values into a matrix. This is the rotation matrix
R
equivalent to the quaternion. Method
for conversion from quaternion to matrix can be found in [23]. The translation can be found in a similar way as in the SVD method (cf. Eq. 3.13). 30
3.6.3
Computing Transformation by Approximation
This method does not provide a closed form solution, rather it assumes that all the three angles i.e yaw, pitch and roll are small enough that rst-order Taylor series approximation can be applied [25, 26], i.e.
sin (θ) = θ −
θ3 θ5 + − ... ≈ θ 3 5
(3.15)
cos (θ) = 1 −
θ2 θ4 + − ... ≈ 1 2 4
(3.16)
and
Computing the net rotation matrix by multiplying the individual rotation matrices around
x, y
and
z
axis gives
R = Rx · Ry · Rz
=
cos (θx ) cos (θy ) cos (θx ) sin (θy ) sin (θz ) − sin (θx ) cos (θz ) sin (θx ) cos (θy ) sin (θx ) sin (θy ) sin (θz ) + cos (θx ) cos (θz ) . . . −sin (θy ) cos (θy ) sin (θz )
cos (θx ) sin (θy ) cos (θz ) + sin (θx ) sin (θz ) sin (θx ) sin (θy ) cos (θz ) − cos (θx ) sin (θz ) cos (θy ) cos (θz )
...
(3.17) After applying the approximations of eq. 3.15 and 3.16 yields
1 R ≈ θx −θy
θy θz − θx θx θy θz + 1 θz
θy + θx θz θx θy − θz 1
(3.18)
Since we are assuming small angles, so we make another assumption that multiplication of two or more angles will give a smaller term which can be neglected as well.
1 R ≈ θx −θy
−θx 1 θz
θy −θz 1
(3.19)
Eq. 3.19 can be viewed as a sum of an Identity matrix and a skew-symmetric matrix.
0 R ≈ I(3×3) + θx −θy 31
−θx 0 θz
θy −θz 0
(3.20)
In order to minimize the error function in eq. 3.1 we need to minimize the term i.e.
(R · pi + t) − qi = 0
(3.21)
We can solve eq. 3.21 for rotation angles rst by substituting the value of translation vector
t
from eq. 3.13
(R · pi + cT − RcS ) − qi = 0
(3.22)
rearranging terms in eq. 3.22
R · (pi − cS ) − (qi − cT ) = 0
(3.23)
R · p0i − qi0 = 0
(3.24)
Substituting the value of
0 qi,x 0 qi,y 0 qi,z
R
from eq. 3.20 to eq. 3.24 yields
p0i.x 0 = p0i,y + θx p0i,z −θy
−θx 0 θz
0 pi.x θy −θz p0i,y 0 p0i,z
(3.25)
Eq. 3.25 can be rewritten as
0 qi,x 0 qi,y 0 qi,z
0 p0i.x p0i,y − p0i,z p0i,z −p0i,y
=
−p0i,z 0 p0i,x
p0i,y θz −p0i,x θy θx 0
Eq. 3.26 can be used to solve for the yaw, pitch and roll angles
(3.26)
(θx , θy , θz ).
The translation can be found in a similar way as we did in the SVD method (cf. Eq. 3.13). Since its an approximate solution but as we are using it in every iteration of ICP algorithm, so in the limiting case, after ever step the dierence in the yaw, pitch and roll angles approaches to zero, hence the approximate solution will approach exactness.
32
Chapter 4 Discussion 4.1
FastSLAM Versus 6-D SLAM
Both FastSLAM and 6-D SLAM have their advantages and disadvantages. The type of algorithm one should choose depends on the application. If the task of the robot is to navigate through a path having dierent obstacles then high accuracy in robot and feature location will be required. FastSLAM should be used in this case. In cases when the task of the robot is to locate dierent objects in an environment and manipulate them in a desired way, then a highly dense representation of the object and the environment in which the object is located is required. In this case, 6-D SLAM should be used. Figure 4.1 shows maps built with FastSLAM (left) and 6-D SLAM (right) overlaid with the planes of the actual kitchen environment (cf.
Figure 2.3)
reconstructed manually. It can be seen that the map built with FastSLAM is more accurate but at the same time is sparse as well.
Figure 4.1: Maps built with FastSLAM (left) and 6-D-SLAM (right)
If we compare Figures 2.4 and 2.5 with Figures 3.5 and 3.6 we can see that the map built using FastSLAM is sparse but it does not have noisy values. Figure 4.2 shows a case where the ICP algorithm failed to converge correctly. Frustum ICP is used which has the maximum chances of correct convergence 33
because the points are transformed using the odometric data rst. Compared to gure 2.8 it can be seen that usage of FastSLAM is better in this case.
Figure 4.2: Reconstruction of table using Frustum ICP
34
Finally a comparison has been made by reconstructing the surface of 3D map of the kitchen built using FastSLAM and 6-D SLAM. Figure 4.3 shows the planes of the actual kitchen environment reconstructed manually.
Figure 4.4
shows the reconstructed surfaces of maps from FastSLAM (left) and 6-D SLAM (right).
Figure 4.3: Reconstructed planes of kitchen environment
Figure 4.4: Reconstructed surface of kitchen map built using FastSLAM (left) and 6-D SLAM (right)
35
Table 4.1 enlists a few pros and cons of FastSLAM and 6-D SLAM. FastSLAM
6-D SLAM
Models the pose of the robot and the
Does not use any noise model.
features as a Gaussian model.
more like an averaging technique.
Impractical to work without odomet-
Can work without odometric data as
ric data because it will require a lot
well.
of particles and a very large noise co-
ing into wrong convergence basin is
variance.
slightly increased.
It is
But the chances of converg-
Corrects the robot and the feature pose in a statistical way and gives the
Only corrects the robot pose.
best possible map. Error does not propagate in every step.
Error propagates in every step.
Table 4.1: Comparison of FastSLAM and 6-D SLAM
4.2
Conclusion and Future Work
Both FastSLAM and 6-D SLAM are implemented and tested under dierent real world scenarios. Information is processed at pixel level because of the usage of color and time-of- ight camera.
Maps are built which are then used by the
robot to navigate and perform manipulation tasks. In future information from color images will be exploited further. Pre ltering will be done to remove the noisy data from the time-of- ight camera. As discussed in [32], FastSLAM will be extended further to work for dynamic environments as well. In the past decade, a lot of variants of ICP algorithms have been introduced. A nice comparison of these variants can be found in [31]. These variants will also be tested and compared.
36
Bibliography [1]
R.C. Smith and P. Cheeseman.,
"On the representation and estima-
tion of spatial uncertainty", International Journal of Robotics Research, 5(4):5668, 1986. 1, 3 [2]
Michael M., Sebastian T., Daphne K., Ben W.,"FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem", In: AAAI National Conference on Arti cial Intelligence, Edmonton, Canada, 2002. 1, 3
[3]
Andreas Nütcher, 3D Robotic Mapping, The Simultaneous Localization and Mapping Problem with Six Degrees of Freedom, Springer Tracts in Advanced Robotics, Vol 52,2009. 1
[4]
P. Besl and N.D McKay, "A Method for Registration of 3-D Shapes", Proc. of IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 14, No. 2, pp.239-256, 1992. 1
[5]
Gordon
N.J.,
Salmond
D.J.
and
Smith
A.F.M.
"Novel
approach
to
nonlinear/non-Gaussian Bayesian state estimation", IEE-Proceedings-F, 140, 107113, 1993. 4 [6]
Doucet, A., Johansen, A.M., "A tutorial on particle ltering and smoothing: fteen years later", Technical report, Department of Statistics, University of British Columbia, December 2008. 4, 18
[7]
Herbert Bay, Andreas Ess, Tinne Tuytelaars, Luc Van Gool "SURF: Speeded Up Robust Features", Computer Vision and Image Understanding (CVIU), Vol. 110, No. 3, pp. 346359, 2008. 5
[8]
Michael M. and Sebastian T., FastSLAM, A Scalable Method for the Simultaneous Localization and Mapping in Robotics, Springer Tracts in Advanced Robotics, Vol 27, pp 71. 5, 7
[9]
Steven
M.,
"Planning
Algorithms",
http://planning.cs.uiuc.edu/node102.html [Accessed:
Available:
July 2010],
Cam-
bridge University Press, 2006. 15 [10] PJ Besl and HD McKay, A method for registration of 3-d shapes , IEEE Transactions on pattern analysis and machine intelligence, vol. 14, no. 2, pp. 239-256, 1992. 16 37
[11] Bali Y. Chen and G.Medioni, "Object Modeling by Registration of Multiple Range Images", Image and Vision Computing, Vol. 10, no. 3, pp. 145-155, April 1992. 16 [12] Nüchter A., Surmann H., Lingemann K., Hertzberg J., Thrun, S., "6D SLAM with an Application in autonomous mine mapping", In: Proceedings of the IEEE International Conference on Robotics and Automation, New Orleans, USA, pp. 19982003 (April 2004). 16 [13] G. Sharp, S. Lee, and D. Wehe, "ICP registration using invariant features", IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE INTELLIGENCE, VOL. 24, NO. 1, pp. 90-102, (January 2002). 16 [14] A. E. Johnson and S. B. Kang,"Registration and integration of textured 3-D data", in NRC '97, Proceedings of the International Conference on Recent Advances in 3-D Digital Imaging and Modeling, (1997). 16 [15] Z.Zhang, "Iterative Point Matching for Registration of Free-Form Curves", Tech. Rep. RR-1658. 19 [16] Lengyel, E. "A fast cylinder-frustum intersection test", In Game programming gems. Boston, MA: Charles River Media, 2000. 27 [17] S. May, D. Droeschel, D. Holz, S. Fuchs, E. Malis, A. Nüchter, and J. Hertzberg. "Three-dimensional mapping with time-of- ight cameras," Journal of Field Robotics, Special Issue on Three-Dimensional Mapping, Part 2, 26(11-12):944945, (December 2009). 27 [18] Arun, K.S., Huang, T.S., Blostein, S.D.: Least square tting of two 3-d point sets, IEEE Transactions on Pattern Analysis and Machine Intelligence 9(5), 698700 (1987). 30 [19] Andreas Nütcher, 3D Robotic Mapping The Simultaneous Localization and Mapping Problem with Six Degrees of Freedom, Springer,pp 38-40 (2008). 30 [20] Horn, B.K.P.:
Closedform solution of absolute orientation using unit
quaternions, Journal of the Optical Society of America A 4(4),pp 629642 (1987). 30 [21] Hamilton, W.: On a new Species of Imaginary Quantities connected with a theory of Quaternions. In:
Proceedings of the Royal Irish Academy,
Dublin, Ireland (November 1843) 30 [22] Andreas Nütcher, 3D Robotic Mapping The Simultaneous Localization and Mapping Problem with Six Degrees of Freedom, Springer,pp 42-45 (2008). 30 [23] J.M.P. van Waveren, From Quaternion to Matrix and Back, Id Software, Inc, (2005). 30 [24] Lorusso A., Eggert D., Fisher R.: "A Comparison of Four Algorithms for Estimating 3-D Rigid Transformations", In: Proceedings of the 4th British Machine Vision Conference (BMVC 1995), Birmingham, England, pp. 237 246, (September 1995). 29 38
[25] Andreas Nütcher, "3D Robotic Mapping, The Simultaneous Localization and Mapping Problem with Six Degrees of Freedom", pp.50-52. 31 [26] Chuck S., Luis I., CSci 6971: Image Registration, Lecture 18, Available: http://www.cs.rpi.edu/academics/courses/spring04/imagereg/lecture18.ppt, [Accessed: July 2010]. 31 [27] Michael M., Sebastian T., Daphne K., Ben W.,"FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges", In: Proc. of the International Conf. on Arti cial Intelligence (IJCAI), pp. 1151-1153, (2003). 1, 3
FastSLAM, A Scalable Method for the Simultaneous Localization and Mapping in Robotics, Springer Tracts in Advanced
[28] Michael M. and Sebastian T.,
Robotics, Vol 27, pp 79-80. 3 [29] Peer N., Niko S., Peter P., "FastSLAM using SURF Features:
An E -
cient Implementation and Practical Experiences", In: Proc. of the International Conference on Intelligent and Autonomous Systems (IAV), Toulouse, France. 3 [30] Arnaud D., Nando de F., Kevin M., Stuart R., "Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks", In: Proc. of the 16th Conference on Uncertainty in Arti cial Intelligence, (2000). 4 [31] S. Rusinkiewicz and M. Levoy, "E cient variants of the ICP algorithm", 3-D Digital Imaging and Modeling, 2001. Proceedings. Third International Conference on 3D Digital Imaging and Modeling, pp. 145152, 2001. 36
FastSLAM, A Scalable Method for the Simultaneous Localization and Mapping in Robotics, Springer Tracts in Advanced
[32] Michael M. and Sebastian T., Robotics, Vol 27, pp 90-95. 36
39
40
Appendix A List of External Libraries A.1
ANN: A Library for Approximate Nearest Neighbor Searching
ANN is a library written in C++ by David M. Mount and Sunil Arya, which supports data structures and algorithms for both exact and approximate nearest neighbor searching in arbitrarily high dimensions. This library is used to build up the kd-Tree which is used to perform searching in k dimensions. For detailed information and download go to
http://www.cs.umd.edu/~mount/ANN/
41
A.2
Bayesian Filtering Library
The Bayesian Filtering Library (BFL) is a C++ library provided by Open Robot Control Software (OROCOS). This library is used for matrix operations and implementation of probabilistic methods. For detailed information and download go to
http://www.orocos.org/bfl
A.2.1
Gaussian Distribution
Gaussian(const MatrixWrapper::ColumnVector &Mu, const MatrixWrapper::SymmetricMatrix &Sigma) Class representing Gaussian distribution. Parameters: Mu: Mean Vector of the Gaussian Sigma: Covariance Matrix of the Gaussian For further information go to
http://people.mech.kuleuven.be/~tdelaet/bfl_doc/doc/html/classBFL_ 1_1Gaussian.html#42213e80e2818ffddf49d6d1abd014a4 A.2.2
Resampling Function
bool SampleFrom (vector&list_samples, const unsigned int num_samples, int method = DEFAULT, void * args = NULL) This function is used for Resampling in FastSLAM algorithm. Parameters: list_samples: list of samples that will contain result of sampling num_samples: Number of Samples to be drawn (iid) method: Sampling method to be used For further information go to
http://people.mech.kuleuven.be/~tdelaet/bfl_doc/doc/html/classBFL_ 1_1Pdf.html#5a889a2c5f3829683cdc5c47f82016f1
42
