SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada...

Defence Research andDevelopment Canada

Recherche et développementpour la défense Canada Canada

SLAM Techniques and Algorithms

Jack Collier

Defence R&D Canada • R & D pour la défense Canada

What will we learn

Gain an appreciation for what SLAM is and canaccomplish

Understand the underlying theory behind SLAM

Understand the terminology and fundamentalbuilding blocks of SLAM algorithms

Appreciate the deficiencies of SLAM and SLAMalgorithms

Won’t focus on the math, but the concept

Online non-linear feature based SLAM withunknown data association

Outline

What is SLAM?

Probabilistic basis of SLAM

EKF SLAM

Data Association

Closing the Loop

FastSLAM

Sub-Mapping SLAM

Resources

Questions?

What is SLAM?

Simultaneous Localization and Mapping

Given an unknown environment and vehicle pose:

Move through the environmentEstimate the robot poseGenerate a map of environmental features

Use the robot pose estimate to improve the maplandmark position estimates

Use the landmark estimates to improve the robotpose estimate

What is SLAM?and why do we need it?

Robot motion models aren’t accurate

Wheel odometry error is cumulative

IMU sensors errors

Maps are generated from sensors on the vehicle. If wecan’t accurately predict the robot pose then how can weproduce an accurate map.

GPS can help but is not always available or reliable.

What is SLAM?The SLAM problem

Given:

Robot control signal uk (or measurement)

A set of feature observations zK (sensormeasurements)

Estimate:

Map of landmarks Mk+1

Robot Pose Vk+1

Sources of Error:

Control signal

Mapping sensor

Motion model

Observation model

Probabilistic Basis of SLAMProbability Theory

Probability Theory gives us a framework for dealingwith these sources of error

The online SLAM theory is:p(Vk+1,Mk+1|zk , uk)

Estimate the Joint Probability of Vk+1 and Mk+1

conditioned on zk , uk (Joint Conditional PDF)

Probabilistic Basis of SLAMSLAM Probability toolkit

Conditional Probability p(x |y) = p(x ,y)p(y)

Product Rule p(x , y) = p(x |y)p(y)

Bayes Rule p(x |y) = p(y |x)p(x)p(y)

p(x |y) = ηp(y |x)p(x)

Gaussian PDFp(x) = det(2πΣ)−

12 exp−1

2(x − x̄)T Σ−1(x − x̄)

Probabilistic Basis of SLAMSLAM as a Bayesian filter

p(Vk+1,Mk+1|zk+1, uk+1) =

ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)

∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk

p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter)

=ηp(zk+1|Vk+1,Mk+1)

p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)

= ηp(zk+1|Vk+1,Mk+1)∫

p(Vk+1|Vk , uk)p(Vk ,Mk |zk , uk)dvk

No closed form solution

Approximate the solution using an Extended Kalman Filter

= ηp(zk+1|Vk+1,Mk+1)∫

p(Vk+1|Vk , uk)p(Vk,Mk|zk,uk)dvk

Prior probability:Prior state and covariance estimates from the last filter iteration

= ηp(zk+1|Vk+1,Mk+1)∫

p(Vk+1|Vk,uk)p(Vk ,Mk |zk , uk)dvk

Motion Model:Probabilistic motion model estimates the new vehicle pose covari-ance estimates from the prior estimate and the control

= ηp(zk+1|Vk+1,Mk+1)∫

p(Vk+1|Vk , uk)p(Vk ,Mk |zk , uk)dvk

Measurement Model:Measurement model gives the expected value of the feature obser-vations.

Probabilistic Basis of SLAMGaussian PDF

EKF SLAM

Simultaneous Localization and Mapping:

Joint estimate both robot pose and position ofunique landmarks

Landmark estimates improve robot pose estimatesand vice versa

Use standard Extended Kalman Filtering techniques

Assumes Gaussian noise and error.

ym1...

, Vk =

EKF SLAMAlgorithm

), Pk =

(Pv Pvm

PTvm Pm

Prediction - A prediction of the new state vector andcovariance matrix is calculated from the previous stateand covariance, and the new control uk .

EKF SLAMAlgorithm

), Pk =

(Pv Pvm

PTvm Pm

Data Association - Find matches between the currentlandmarks Mk and the new set of observed features zk .

EKF SLAMAlgorithm

), Pk =

(Pv Pvm

PTvm Pm

Measurement Update - Calculate the Kalman gain foreach observed landmark and update the state and covari-ance values based on that Kalman gain and the measure-ment innovation.

EKF SLAMAlgorithm

), Pk =

(Pv Pvm

PTvm Pm

Augmentation - Integrate newly observed landmarks intothe state vector and covariance matrix.

EKF SLAMLinearization

Kalman Filters work under a linear Gaussianassumption

Robot motion and measurement functions arenon-linear

Use Taylor Expansion to linearize (Approximate aGaussian)g ≈ g + g ′

g ′ is Jacobian of the function with respect to itsvariables

EKF SLAMLinearization

EKF SLAMPrediction

Predict the robot motion from the control signal

Linearize to approximate the Gaussian

V̄k+1 = Vk + ∆V

EKF SLAMPrediction

Vk+1 =

− vk

ωksin(θr

k) + vk

ωksin(θr

k + ωk∆t)vk

ωkcos(θr

k)− vk

ωkcos(θr

k + ωk∆t)

ωk∆t

EKF SLAMPrediction

P̄v(k+1) = GkPv(k)GTk

P̄vm(k+1) = RkQRTk

P̄k+1 =

(P̄v(k+1) P̄vm(k+1)

P̄Tvm(k+1) Pm

G linearizes the state transition functionR maps additional motion noise into the state space

EKF SLAMFeature Extraction Algorithms

Extract stable salient features from environment):

What do we want in feature extraction algorithm?

Stable features

Outlier rejection

Accuracy

Expectation Maximization

RANSAC

Split and Merge

Hough Transform

Incremental Line Fitting

EKF SLAMLaser Feature Extraction

Extract features from Raw laserdata

Incremental algorithm (linetracking)

Fit a line to a set of pointsCompute the residualIf below error threshold addanother point and repeatIf above the last point addedis a feature

EKF SLAMData Association

Find matches between features and landmarks

For each feature

Calculate the predicted feature for each landmark(measurement model)compute the Mahalanobis Distance:choose the feature/landmark with the lowestdistance (Maximum Likelihood) below somethreshold

End for

Measurement Model:

( √∆y 2 + ∆x2

arctan (∆y ,∆x)

Mahalanobis Distance normalizes feature/landmarkdistance based on their covariances:

dij = (zi − zmj )T (Hj

¯Pk+1HjT + R)−1(zi − zm

Measurement Model:

( √∆y 2 + ∆x2

arctan (∆y ,∆x)

)Mahalanobis Distance normalizes feature/landmarkdistance based on their covariances:

dij = (zi − zmj )T (Hj

¯Pk+1HjT + R)−1(zi − zm

EKF SLAMWhy Mahalanobis Distance?

Why Mahalanobis Distance:

Points which lie on the same covariance ellipse have areequidistant

EKF SLAMUpdate

For each data association

Calculate Kalman Gain:Kj = P̄k+1Hj

T (Hj P̄k+1HjT + R)−1

H linearizes the measurement modelUpdate state and covariance estimates:Xk+1 = X̄k+1 + Kj(zi − zm

Pk+1 = P̄k+1 − KjHj P̄k+1

End for

Innovation vector - Larger → bigger correction

Kalman Gain - Weights the innovation vector.Smaller measurement error → higher gain (trustthe measurement)Smaller covariance → smaller gain (trust theprediction)

EKF SLAMAugment

State Vector and Covariance matrix grow as newlandmarks are observed

If a feature has no matches, add it to the statevector as a new landmark

EKF SLAMAugment

New Landmark Locations:

xmi = x r

k + ri cos(θrk + βi)

ymi = y r

k + ri sin(θrk + βi)

EKF SLAMAugment

Append to state vector:

EKF SLAMAugment

Original Covariance Matrix:

(Pv Pvm

PvmT Pm

EKF SLAMAugment

Augmented Covariance Matrix:

Pv Pvm PvTFi

PvmT Pm Pvm

FiPv FiPvm FiPvFiT + MiRMi

F and M are the Jacobians which linearize the new land-mark equations with respect to vehicle pose and measure-ment variables respectively

EKF SLAMOther Issues

Can use provisional landmark list to reject spuriousfeatures

Pruning (removing old/irrelevant landmarks)

Landmark Signatures (reflectivity/color/shape/etc.)

Landmark minimum distance

Intelligent update (Only update relevant landmarks)

Sub-Mapping (more on this later)

EKF SLAMAdvantages

Straightforward application of the EKF

Large body of research to pull from

Works reasonably well for small number of featuresand distinct landmarks

EKF SLAMDisadvantages

Complexity Quadratic with number of features

No guarantee of convergence in non-linear case

Make hard decisions about data associations

Can’t correct for erroneous data associations

Need sufficiently distinct landmarks (low clutter)

Gaussian Assumption

EKF SLAMVariants

Unscented KF SLAM

Linearization using Sigma pointsPreserves the true mean and covariance of theposterior

Extended Information Filter SLAM

Dual representation of EKFLess complex measurement update

Data AssociationErroneous data association WILL cause SLAM to fail!

1 feature to 2 landmarks

2 features to 1 landmark

clutter

Symmetry

Reduce the maxMahalanobis distance?

Data Association

How do determine the correspondence between a featureand landmark?

Individual Compatibility

Joint Compatibility Branch and Bound

Combined Constrained Data Association

Randomized Joint Compatibility

Multi hypothesis Data Association

Data AssociationIndividual Compatibility

Calculate Mahalanobis distance from each featureto landmark

Choose feature with smallest distance to landmarkwithin threshold (validation gate)

Disregard ambiguous associations

Advantage: Simple

Disadvantage: Can be error prone especially inclutter

Data AssociationJoint Compatibility Branch and

Consider multiple associations simultaneously

Find the largest set of matches which correspond

Joint Mahalanobis distance measurement with jointvalidation gate

Branch and bound interpretation tree search

Advantage: Robust

Disadvantage: Tree search computation/May beAmbiguous sets

PotentialMatchesz1→ L1 or L2z2→ L2 or L3z3→ L4Null

Data AssociationCombined Constrained Data

Association

Correspondence Graph technique

Nodes are individually compatible associations

Edges are compatible associations

Find the maximum clique of associations that aremutually compatible

Consider both relative and absolute constraints

Advantage: Can work with no pose info (relativeconstraints)

Disadvantage: May be ambiguous sets

Data AssociationRandomized Joint Compatibility

JCBB with RANSAC

Uses relative constraints

Randomly select m of the n feature measurements

Advantage: Reduced complexity, no pose necessary

Disadvantage: Don’t always know Pfail and Pgood

Closing the LoopWhat is loop closing?

Even with the best SLAM algorithm poseuncertainty will increase as the vehicle moves.

This pose uncertainty means that landmarklocations further from the map origin have a higheruncertainty

Revisiting a previously observed landmarkssignificantly reduces uncertainty in robot andlandmark pose estimates

Errors in landmark estimates are correlated withrobot pose

New pose info necessarily improved landmarkestimates

Closing the LoopHow to Close the loop

Map Matching Techniques (feature matching)Can use pose estimate to constrain the searchExpectation Maximization AlgorithmNon-linear Optimization

FastSLAMRepresent probability distributions by set of particlesRao-Blackwellized particle filter (low dimensionalEKFs)Each particle maintains its own pose and mapestimates (multi-hypothesis)Each landmark has its own EKFN landmarks and M particles we have MxN + 1filters

FastSLAMAlgorithm

Do M times:

Retrieve pose from particle k

Predict a new pose (motion model)

Data Association

Measurement Update

Importance Weight

ENDRe-sample with Replacement using the importanceweight

Raw Odometry

FastSLAM

FastSLAMAdvantages

Time logarithmic to number of landmarks

Multi-hypothesis data association (robustness)

No linearization of non-linear motion models

Solves both full SLAM and Online SLAM problems

FastSLAMDisadvantages

Throws away correlations

Over optimistic pose estimate

Harder to close the loop

Sub-Mapping SLAM

Partition SLAM into sub-mapsOptimally Fuse at a global level periodicallyReduces EKF linearization errorsReduces computational complexityClosing the loop techniques for sub-maps

Sub-Mapping SLAMAlgorithms

Hybrid-SLAM

Decoupled Stochastic Mapping

Hierarchical SLAM

Conditionally In dependant SLAM

Divide and Conquer

EKF SLAM vs. D&C SLAM

SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada...

Documents

Transcript of SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada...

Pre-Robotic Navigation Identiﬁcation of Pedestrian …simultaneous localization and mapping (i.e., SLAM) and structure from mapping (i.e., SfM) algorithms. In this methodology, we

Slam PowerPoint

Algorithms for Simultaneous Localization and …cseweb.ucsd.edu/~yuc007/documents/re_presen.pdfAlgorithms for Simultaneous Localization and Mapping (SLAM) Yuncong Chen Research Exam

SLAM PSanthanam

Poetry slam

BUSINESS DISRUPTIONS WITH ALGORITHMS, DRONES & ROBOTS · 1 BUSINESS DISRUPTIONS WITH ALGORITHMS, DRONES & ROBOTS SUDHA JAMTHE, CEO IoT Disruptions IoT Slam 2016 April 28 2016 IoTDisruptions.com

SLAM. ProblemRecognition

EGO-SLAM: A Robust Monocular SLAM for Egocentric Videos

Slam 2014 - PDF - Slam on Demand Brochure

Application-oriented Design Space Exploration for …ejohns/...2017_slam_design_space_exploration.pdf · Application-oriented Design Space Exploration for SLAM Algorithms ... design

ATELIER SLAM

A Framework for use in SLAM algorithms Principle Investigator: Shaun Egan Supervisor: Dr. Karen Bradshaw.

SLAM-based Mobile Mapping System · Mobile Mapping System PRODUCT Ellipse-D Inertial Navigation System with embedded RTK GNSS receiver PROJECT Motion Compensation & SLAM Algorithms

Dataset Generation and Benchmarking of SLAM Algorithms for Robotics …urserver.kaist.ac.kr/seminar/2019_ICRA2019_JieumHyun.pdf · 2020-02-13 · Dataset Generation and Benchmarking

Towards Quantitative Comparisons of Robot …robotics.ucmerced.edu/sites/robotics.ucmerced.edu/files/...Towards Quantitative Comparisons of Robot Algorithms: Experiences with SLAM

SLAM Effect

CNN-SLAM: Real-time dense monocular SLAM with learned ...campar.in.tum.de/pub/tateno2017cvpr/tateno2017cvpr.pdf · CNN-SLAM: Real-time dense monocular SLAM with learned depth prediction

SLAM Algorithm · Flowchart SLAM Algorithm There isn’t ’the’ SLAM algorithm SLAM is just a problem, but luckily there a possibilities to solve it Albin Frischenschlager, 0926427

Modeling Perceptual Aliasing in SLAM via Discrete ... · model perceptual aliasing and outlier correlation in SLAM. We proposea novel approachto obtain provably-robust SLAM algorithms:

SLAM Accelerated Using Hardware to improve SLAM algorithm performance.