![Page 1: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/1.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Single-camera SLAM using the SceneLiblibrary
Paul Smith
Robotics Research Group Talk, Nov 2005
![Page 2: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/2.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Outline
1 IntroductionThe Camera as a Position SensorVisual SLAM
2 Davison’s MonoSLAMOverview and NomenclatureExtended Kalman FilterAutomatic Map ManagementPerformance
3 The SceneLib LibrariesIntroductionThe Scene LibraryThe MonoSLAM LibraryApplications using SceneLib
4 Final ThoughtsFinal Thoughts
![Page 3: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/3.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The Camera as a Position Sensor
AimUse a camera as a position sensor
Challenges
Monocular (no depth)UnconstrainedHigh acceleration & large rotationsUsually want real-time localisation
![Page 4: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/4.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Off-line Processing
DataDataData
Structure from motionTypical Computer vision approachBundle adjustment over a long sequenceApplied to post-production, 3D model reconstruction.
![Page 5: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/5.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Sequential Real-time Processing
Data Data
Simultaneous Localisation and Mapping (SLAM)Typical robotics approach.Building a long-term map by propagating and correctinguncertaintyMostly used in simplified 2D environments with specialisedsensors such as laser range-finders.
![Page 6: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/6.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Classic Approaches to Visual SLAM
Davison, ICCV 2003Traditional SLAM approach (Extended Kalman Filter)Maintains full camera and feature covarianceLimited to Gaussian uncertainty only
Nistér, ICCV 2003Structure-from-motion approach (Preemptive RANSAC)Frame-to-frame motion onlyDrift: No repeatable localisation
![Page 7: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/7.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Classic Approaches to Visual SLAM
Davison, ICCV 2003Traditional SLAM approach (Extended Kalman Filter)Maintains full camera and feature covarianceLimited to Gaussian uncertainty only
Nistér, ICCV 2003Structure-from-motion approach (Preemptive RANSAC)Frame-to-frame motion onlyDrift: No repeatable localisation
![Page 8: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/8.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Recent Approaches to Visual SLAM
Pupilli & Calway, BMVC 2005Traditional SLAM approach (Particle Filter)Greater robustness: handles multi-modal casesNew features not rigorously initialised
Eade & Drummond, 2006FastSLAM approach (Particle Filter/Kalman Filter)Particle per camera hypothesis, Kalman filter for featuresAllows larger maps: update O(M log N) instead of O(N2)
![Page 9: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/9.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Recent Approaches to Visual SLAM
Pupilli & Calway, BMVC 2005Traditional SLAM approach (Particle Filter)Greater robustness: handles multi-modal casesNew features not rigorously initialised
Eade & Drummond, 2006FastSLAM approach (Particle Filter/Kalman Filter)Particle per camera hypothesis, Kalman filter for featuresAllows larger maps: update O(M log N) instead of O(N2)
![Page 10: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/10.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Outline
1 IntroductionThe Camera as a Position SensorVisual SLAM
2 Davison’s MonoSLAMOverview and NomenclatureExtended Kalman FilterAutomatic Map ManagementPerformance
3 The SceneLib LibrariesIntroductionThe Scene LibraryThe MonoSLAM LibraryApplications using SceneLib
4 Final ThoughtsFinal Thoughts
![Page 11: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/11.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Davison’s MonoSLAM: Overview
Main featuresInitialisation with known targetExtended Kalman Filter
’Constant velocity’ motion modelImage patch features with ActiveSearch
Automatic Map MeasurementParticle filter for initialisation of newfeatures
![Page 12: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/12.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Nomenclature
The cameraThe camera position state xp is its 3Dposition and orientation
xp =
(rW
qWR
)=
xyzq0qxqyqz
The camera state xv contains xp plusoptional additional state information(e.g. velocity and angular velocity)
![Page 13: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/13.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Nomenclature
FeaturesEach feature y i is a 3D position vector
y i =
xiyizi
![Page 14: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/14.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Nomenclature
System State
x =
xvy1y2...
P =
Pxx Pxy1 Pxy2 . . .Py1x Py1y1 Py1y2 . . .Py2x Py2y1 Py2y2 . . .
......
.... . .
PDF over camera and feature state is modelled as a singlemulti-variate Gaussian and we can use the ExtendedKalman Filter.
![Page 15: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/15.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Prediction Step
Time Update1 Estimate new location2 Add process noise
![Page 16: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/16.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Prediction Step
Time Update1 Estimate new location2 Add process noise
![Page 17: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/17.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Prediction Step
Time Update1 Estimate new location2 Add process noise
![Page 18: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/18.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Prediction Step
Time Update1 Project the state ahead
xnew = f (x , u)
2 Project the error covariance ahead
Pnew =∂f∂x
P∂f∂x
T+ Q
![Page 19: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/19.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Motion models
Constant velocityAssume bounded, Gaussian-distributed linear and angularacceleration
xnew =
rW
newqWR
newvW
newωR
new
= f (x , u) =
rW +
(vW + V W
)∆t
qWR × q((
ωR + ΩR)∆t
)vW + V W
ωR + ΩR
![Page 20: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/20.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Update Step
Measurement Update1 Measure feature(s)2 Update positions and uncertainties
![Page 21: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/21.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Update Step
Measurement Update1 Measure feature(s)2 Update positions and uncertainties
![Page 22: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/22.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Update Step
Measurement Update1 Measure feature(s)2 Update positions and uncertainties
![Page 23: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/23.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Extended Kalman Filter: Update Step
Measurement Update1 Make measurement z to give the innovation ν
ν = z − h (x)
2 Calculate innovation covariance S and Kalman gain W
S =∂h∂x
P∂h∂x
T+ R
W = P∂h∂x
TS−1
3 Update estimate and error covariance
xnew = x + Wν
Pnew = P− WSWT
![Page 24: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/24.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Measurement Step: Image Features and the Map
Feature measurements are the locations ofsalient image patches.Patches are detected once to serve aslong-term visual landmarks.Sparse set of landmarks gradually accumulated andstored indefinitely.
![Page 25: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/25.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Measurement Step: Active Search
Active search within elliptical search regions defined by thefeature innovation covariance.Template matching via exhaustive correlation search.
![Page 26: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/26.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Automatic Map Management
Initialise system from a few known features.Add a new feature if number of visible features drops belowa threshold (e.g. 12).Choose salient image patch from a search box in anunderpopulated part of the image.
![Page 27: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/27.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 28: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/28.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 29: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/29.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 30: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/30.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 31: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/31.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 32: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/32.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 33: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/33.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 34: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/34.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small,
![Page 35: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/35.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Monocular Feature Initialisation with Depth Particles
A new feature has unknown depth1 Populate the line with 100 particles, spaced uniformly
between 0.5m and 5m from the camera.2 Match each particle in successive frames to find probability
of that depth.3 When depth covariance is small, convert to Gaussian.
![Page 36: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/36.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Feature initialisation
New features need adding to the state vector andcovariance matrix.
Increasing the state size dynamically
xnew =
xvy1y2y i
P =
Pxx Pxy1 Pxy2 Pxx
∂y i∂xv
T
Py1x Py1y1 Py1y2 Py1x∂y i∂xv
T
Py2x Py2y1 Py2y2 Py2x∂y i∂xv
T
∂y i∂xv
Pxx∂y i∂xv
Pxy1∂y i∂xv
Pxy2∂y i∂xv
Pxx∂y i∂xv
T+
∂y i∂hG
RL∂y i∂hG
T
![Page 37: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/37.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Feature deletion
Delete a feature if more than half of attemptedmeasurements fail.
Reducing the state size dynamically
xvy1y2y3
→
xvy1y3
Pxx Pxy1 Pxy2 Pxy3
Py1x Py1y1 Py1y2 Py1y3
Py2x Py2y1 Py2y2 Py2y3
Py3x Py3y1 Py3y2 Py3y3
→Pxx Pxy1 Pxy3
Py1x Py1y1 Py1y3
Py3x Py3y1 Py3y3
![Page 38: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/38.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Example Sequence
![Page 39: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/39.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
What Enables It To Run In Real-time?
Timings
Image loading and administration 1msImage correlation searches 2msKalman Filter update 1msFeature initialisation search 3msGraphical rendering 5msTotal 12ms
Easily manages 30Hz processing on a 3.4GHz desktop PCusing C++, Linux, OpenGL
Main time-saving featuresAutomatic map management criteria to maintain asufficient but sparse mapActive search guided by uncertainty
![Page 40: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/40.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
What Enables It To Run In Real-time?
Timings
Image loading and administration 1msImage correlation searches 2msKalman Filter update 1msFeature initialisation search 3msGraphical rendering 5msTotal 12ms
Easily manages 30Hz processing on a 3.4GHz desktop PCusing C++, Linux, OpenGL
Main time-saving featuresAutomatic map management criteria to maintain asufficient but sparse mapActive search guided by uncertainty
![Page 41: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/41.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Outline
1 IntroductionThe Camera as a Position SensorVisual SLAM
2 Davison’s MonoSLAMOverview and NomenclatureExtended Kalman FilterAutomatic Map ManagementPerformance
3 The SceneLib LibrariesIntroductionThe Scene LibraryThe MonoSLAM LibraryApplications using SceneLib
4 Final ThoughtsFinal Thoughts
![Page 42: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/42.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The SceneLib Libraries
A complete basic Davison MonoSLAM systemWritten in standard C++Three libraries and an application
SceneLib A generic SLAM library. Base classes formotion models, features, measurements andthe Kalman Filter.
SceneImproc Image processing for MonoSLAM(i.e. feature detection and correlation)
MonoSLAM Specific motion and feature-measurementmodels for single-camera SLAM, and acontrol class.
MonoSLAMGlow Application based on GLOW/GLUTlibrary which uses the libraries.
![Page 43: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/43.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Obtaining the SceneLib libraries
All files available from the Active Vision CVS repositoryWill also need VW34 library (soon to be VW35)
Getting and building files from CVScvs -d/data/lav-local/common/cvsroot co VW34cvs -d/data/lav-local/common/cvsroot co SceneLibcvs -d/data/lav-local/common/cvsroot co MonoSLAMGlowcd VW34./bootstrap./configuremakecd ../SceneLib./configuremakecd ../MonoSLAMGlowmake
![Page 44: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/44.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Running the MonoSLAMGlow application
Running MonoSLAMGlow./scenerob
Comment out -D_REALTIME_ in Makefile to usepreviously saved image sequence
![Page 45: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/45.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
SceneLib Documentation
Making and viewing documentationcd SceneLibmake docsfirefox html/index.html
See also SceneLib/Docs/models.tex.
![Page 46: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/46.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The Scene library
Scene is a generic SLAM library.
Main Scene classesScene_Single Stores the full system state and manages
features.Feature Stores and manages a feature’s state vector and
covariancesMotion_Model Base class for all motion models.Feature_Measurement_Model Base class for all feature and
measurement models.Sim_Or_Rob Base class for something that can make
measurements.Kalman Friend class of Scene_Single that implements a
Kalman filter.
![Page 47: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/47.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The Scene library
The main Scene classes are completely generic:Scene_Single stores the ’robot’ state vector xv ,
covariance Pxx and a list of Features.Feature stores a feature state vector y i , it’s
covariances Pxv yi , Pyi yi and Pyi yj (∀j < i).Kalman updates the state given measurements via
Sim_Or_Rob.The Motion_Model and Feature_Measurement_Modelclasses access and interpret the state.Classes in MonoSLAM derived from these base classesmake the application specific.
![Page 48: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/48.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Motion model classes
A Motion_Model knows how to perform the state updatexvnew = f v (xv , u)
It stores no state itself
Motion_Model
TwoD_Motion_Model ThreeD_Motion_Model
Impulse_ThreeD_Motion_Model Zero_ThreeD_Motion_Model
OneD_Motion_Model
Main functionsfunc_fv_and_dfv_by_dxv(xv,u,delta_t) Calculates the new
camera state xvnew and Jacobian ∂f v∂xv
func_Qi(xv, u, delta_t) Calculates the covariance Qi of theprocess noise.
![Page 49: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/49.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Feature measurement classes
A Feature_Measurement_Model knows how to manage afeature’s state and predict a measurement hi = h (y i , xp)
It stores no state itselfSubclassed into two special types:Fully_Initialised_Feature_Measurement_Model A
complete feature in the SLAM map.Partially_Initialised_Feature_Measurement_Model A
feature currently being initialised.
Feature_Measurement_Model
Fully_Initialised_Feature_Measurement_Model
Fully_Init_Wide_Point_Feature_Measurement_Model
Partially_Initialised_Feature_Measurement_Model
Line_Init_Wide_Point_Feature_Measurement_Model
![Page 50: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/50.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Fully-initialised Features
A Fully_Initialised_Feature_Measurement_Modelunderstands the state of a full feature and determines howit is measured.It stores no state itself.
Main functionsfunc_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi, xp) Calculates
the expected measurement vector hi and itsJacobians ∂hi
∂xpand ∂hi
∂y i.
func_Ri(hi) Calculates the measurement noise Ri .visibility_test(xp, yi, xp_orig, hi) Decides whether a feature
should be measured.func_Si(Pxx, Pxyi, Pyiyi, dhi_by_dxv, dhi_by_dyi, Ri)
Calculates the innovation covariance Si .
![Page 51: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/51.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Partially-initialised Features
A Partially_Initialised_Feature_Measurement_Modelhandles a feature which still has some free parameters λ.Each partially-initialised feature references aFeatureInitInfoFeatureInitInfo stores a vector of Particles representingpossible λs and their probabilities.A Partially_Initialised_Feature_Measurement_Model canconvert its feature into a fully-initialised feature.
![Page 52: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/52.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Partially-initialised Features
Main functionsfunc_hpi_and_dhpi_by_dxp_and_dhpi_by_dyi(yi, xp, lambda)
Calculates the expected measurement vector hiand its Jacobians, given values of the freeparameters λ.
func_Ri(hi) Calculates the measurement noise Ri .visibility_test(xp, yi, xp_orig, hi) Decides whether a feature
should be measured.func_Si(Pxx, Pxyi, Pyiyi, dhi_by_dxv, dhi_by_dyi, Ri)
Calculates the innovation covariance Si .func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(ypi, lambda)
Converts a partially-initialised into a fully-initialisedfeature.
![Page 53: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/53.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The MonoSLAM library
The MonoSLAM library providesSpecialisations of Scene base classes:
Impulse_ThreeD_Motion_Model andZeroOrder_ThreeD_Motion_Model motion models.Fully_Init_Wide_Point_Feature_Measurement_Model andLine_Init_Wide_Point_Feature_Measurement_Model.Robot (derived from Sim_Or_Rob) to handle image featuremeasurement.
A MonoSLAM class to provide the main interface.Functions to draw the two graphical displays.
![Page 54: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/54.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The MonoSLAM Class
The MonoSLAM class provides the basic SLAMfunctionality
Main functionsGoOneStep(image, delta_t, currently_mapping_flag) Step the
system onto the next frame. (image is ignored,and instead it must be set usingScene_Single::load_new_image()).
The MonoSLAMInterface class provides full control andfeedback functions.
Main functionsGetScene() Get the Scene_Single class.GetRobot() Get the Robot class.
plus >40 other Get() and Set() functions.
![Page 55: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/55.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
The MonoSLAMGlow Application
Failed matchSuccessful match
Unused feature
![Page 56: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/56.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Other MonSLAM Applications
![Page 57: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/57.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Live Demonstration
![Page 58: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/58.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Outline
1 IntroductionThe Camera as a Position SensorVisual SLAM
2 Davison’s MonoSLAMOverview and NomenclatureExtended Kalman FilterAutomatic Map ManagementPerformance
3 The SceneLib LibrariesIntroductionThe Scene LibraryThe MonoSLAM LibraryApplications using SceneLib
4 Final ThoughtsFinal Thoughts
![Page 59: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/59.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Final Thoughts: EKF-based Visual SLAM
ObservationsThe Davison visual SLAM system works!It works reliably enough for a live demo.It needs no real hidden tricks needed to make it work.
DiscussionNeed more, better features to track
Faster initialisation (fewer particles?)Use full-frame fast feature detection
Better initialisation: how do we deal with points at infinity?Motion model: How do we get smoother, better tracks?Loop closing
![Page 60: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/60.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Final Thoughts: EKF-based Visual SLAM
ObservationsThe Davison visual SLAM system works!It works reliably enough for a live demo.It needs no real hidden tricks needed to make it work.
DiscussionNeed more, better features to track
Faster initialisation (fewer particles?)Use full-frame fast feature detection
Better initialisation: how do we deal with points at infinity?Motion model: How do we get smoother, better tracks?Loop closing
![Page 61: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/61.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Final Thoughts: The SceneLib library
ObservationsThe SceneLib libraries are (reasonably) well-designed and(reasonably) well-documentedThey make it easy to write a Davison-style visual SLAMapplication
DiscussionAre they useful to the Active Vision group?Can we all use them and get the benefits in code sharingthat that will bring?Can we at least all use the same nomenclature andcolours?
![Page 62: Single-camera SLAM using the SceneLib library - doc.ic.ac.ukajd/Scene/Release/monoslamtutorial.pdf · Single-camera SLAM using the SceneLib library Paul Smith Robotics Research Group](https://reader034.vdocuments.net/reader034/viewer/2022042219/5ec5ac3c7f364e37214f8429/html5/thumbnails/62.jpg)
Introduction Davison’s MonoSLAM The SceneLib Libraries Final Thoughts
Final Thoughts: The SceneLib library
ObservationsThe SceneLib libraries are (reasonably) well-designed and(reasonably) well-documentedThey make it easy to write a Davison-style visual SLAMapplication
DiscussionAre they useful to the Active Vision group?Can we all use them and get the benefits in code sharingthat that will bring?Can we at least all use the same nomenclature andcolours?