A.Agha-mohammadi,S.Chakravorty,N.M.Amato

RFM-SLAM:ExploitingRelativeFeatureMeasurementstoSeparateOrientationand

PositionEstimationinSLAM

Authors:SauravAgarwal*,VikramShreeandSumanChakravorty

EDPLab:http://edplab.org

-120 -100 -80 -60 -40 -20 0 20 40 60 80X (m)

-50

0

50

100

Y(m

)

GTSAM Estimate

-120 -100 -80 -60 -40 -20 0 20 40 60X (m)

-50

0

50

100

Y(m

)

RFM-SLAM Estimate

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 2

Introduction:FullSLAM

Driverobotaroundenvironment

Storesensordata(odometry,range-bearingetc.)

Solvenon-linearoptimizationproblem

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 3

TheProblem

Culprit: Odometry-basedguesstoinitializeoptimizercanbearbitrarilybad

Non-linearoptimization-basedSLAMsolversoftengetstuckinlocalminima

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 4

ExampleProblem:2DFeature-basedSLAM

GroundTruth

Odometry-basedInitialGuess

GTSAMEstimate

-120 -100 -80 -60 -40 -20 0 20 40 60 80X (m)

-60

-40

-20

0

20

40

60

80

100

Y(m

)

-150 -100 -50 0 50 100X (m)

-100

-80

-60

-40

-20

0

20

40

60

80

100

Y(m

)

Odometery Estimate

-150 -100 -50 0 50 100X (m)

-100

-80

-60

-40

-20

0

20

40

60

80

100

Y(m

)

GTSAM Estimate

Traditionalapproachmayleadtonon-robustestimates

CatastrophicFailure

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 5

TheSolution:SeparateOrientationEstimation

Computerelativeorientationconstraintsbetweenposes

SolveOn-Manifoldorientationoptimizationproblem

Solvelinearleastsquaresproblemforpositionestimates

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 6

Result:RFM-SLAM

RFM-SLAMavoidscatastrophicfailure

-120 -100 -80 -60 -40 -20 0 20 40 60 80X (m)

-60

-40

-20

0

20

40

60

80

100

Y(m

)

GroundTruth

Odometry

RFM-SLAMEstimate

-150 -100 -50 0 50 100X (m)

-100

-80

-60

-40

-20

0

20

40

60

80

100

Y(m

)

Odometery Estimate

-150 -100 -50 0 50 100X (m)

-100

-80

-60

-40

-20

0

20

40

60

80

100

Y(m

)

RFM-SLAM Estimate

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 7

KeyTakeaway

SeparateOrientationandPositionEstimationforRobustSLAMSolutions

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 8

Method:Step1

Robot

FeaturesRelativeDisplacementfromRobottoFeature

RelativeDisplacementfromFeaturetoFeature

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 9

Method:Step2

SetupRelativeorientationconstraintfromfeaturetofeaturemeasurements

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 10

Method:Step3

Solveon-manifoldoptimizationusingMANOPT*

*Boumal,N.,Mishra,B.,Absil, P.A.andSepulchre,R,“Manopt:aMatlab ToolboxforOptimizationonManifolds”,JournalofMachineLearningResearch,2014,Vol.151455--1459

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 11

Method:Step4

Solvelinearleastsquaresproblemtocomputerobotandlandmarkposition

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 12

Result:Increasingodometrynoise

1 2 3 4

α

0

5

10

15

20

25

30

35

40

RM

SE

(m)

RFM-SLAM β = 1

RFM-SLAM β = 2

RFM-SLAM β = 3

RFM-SLAM β = 4

GTSAM β = 1

GTSAM β = 2

GTSAM β = 3

GTSAM β = 4

⍺:Odometrynoisescalingfactor β:Range-bearingnoisescalingfactor

GTSAMEstimate

RFM-SLAMEstimate

RMSTrajectoryEstimationError

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 13

Result:Increasingrange-bearingnoise

⍺:Odometrynoisescalingfactor β:Range-bearingnoisescalingfactor

1 2 3 4

β

0

5

10

15

20

25

30

35

40

RM

SE

(m)

RFM-SLAM α = 1

RFM-SLAM α = 2

RFM-SLAM α = 3

RFM-SLAM α = 4

GTSAM α = 1

GTSAM α = 2

GTSAM α = 3

GTSAM α = 4

GTSAMEstimate

RFM-SLAMEstimate

RMSTrajectoryEstimationError

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 14

Problem:Non-linearSLAMsolverpronetolocalminima

Solution:Decoupleorientationfromposition

Result:

Accuracydegradesgracefullyasnoisegoesup

EmpiricalresultsshowRFM-SLAMavoidscatastrophicfailure

Impact:UseRFM-SLAMtobootstrapnon-linearsolvers

Summary

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 15

K.Khosoussi,S.Huang,andG.Dissanayake,“Exploitingtheseparablestructureof

slam,”inProceedingsofRobotics:ScienceandSystems,Rome,Italy,July2015.

L.Carlone,R.Aragues,J.A.Castellanos,andB.Bona,“Afastandaccurate

approximationforplanarposegraphoptimization,” TheInternationalJournalof

RoboticsResearch,vol.33,no.7,pp.965–987,2014.

L.Carlone andA.Censi,“Fromangularmanifoldstotheintegerlattice:Guaranteed

orientationestimationwithapplicationtoposegraphoptimization,”IEEE

TransactionsonRobotics,vol.30,no.2,pp.475–492,April2014.

N.Boumal,A.Singer,andP.A.Absil,“Robustestimationofrotationsfromrelative

measurementsbymaximumlikelihood,”in52ndIEEEConferenceonDecisionand

Control,Dec2013.

RelevantRelatedWork

RFM-SLAM@ICRA2017,SingaporeEstimation,DecisionandPlanningLab 16

Software

https://github.com/sauravag/edpl-rfmslam

Estimation,DecisionandPlanningLabwww.edplab.org