Vision-based Dynamic Target Trajectory and Ego-motion ...

Vision-based Dynamic Target Trajectory and Ego-motion

Estimation Using Incremental Light Bundle Adjustment

MichaelChojnackiUnderthesupervisionofAsst.Prof.VadimIndelman

andco-supervisionofProf.EhudRivlin

1

GraduateSeminar,December 2016

M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016

Overview• Motivations• ProblemFormulation• iLBA andDynamicTargetTracking• Optimizationmethod• ExperimentsResults• Conclusions

2


àWhyMotionEstimation?

SpaceExplorationSub-marineexploration

AutonomousDriving IndoorOperation

- AutonomousNavigation

Virtual/AugmentedReality

- Others

PointingDevices

3


àWhyTargetTracking?Surveillance Military

Robot– Humaninteraction

4


Scenario

- Unknownenvironment- Nopriorinformationaboutplatform’strajectory- Nopriorinformationabouttarget’strajectory

althoughmotionmodelisassumed

5

Useofonboardsensors:MonocularCamera

- Interestedinon-lineoperation- NoGlobalPositioningSystem


Scenario6

Efficientlyandsimultaneouslyestimateego-motionandtargettrajectory


Bundle Adjustment (BA) or Simultaneous Localization and Mapping (SLAM)

7

StaticLandmarks

DynamicTarget

SLAM + Detection and Tracking of Moving Object (DATMO)


8

Related Work

• TargetTracking(orDATMO):[Y.Bar-Shalom,1988],[M.Breitenstein,2009]- Assumeknown/highlypredictablesensorlocation

• CombinedSLAMandDATMO:[J.S.Ortega,2007],[C.Wang,2004],[T.D.Vu,2009]- Differenttechniques:EKF,PF,…- Allinvolveoptimizationoverthecamera’sstate,thetarget’sstateandtheobserved3Dstructure!

[www.cs.cmu.edu]

• “Structure-Less”BA:[Steffenetal.,2010],[Indelman,2012]- Allperformbatchoptimization


9

Contributions

• PresentEgo-motionestimationandTargettrackingasacombinedoptimizationproblem

• Integratetargettrackingintoefficient“structure-less”BAframework:Use IncrementalLightBundleAdjustment(iLBA [Indelman etal.,2015])to:

- ImprovecomputationalefficiencycomparedtoBA- Incrementaloptimization:Re-usecalculationsfromprevioussteps

• Showresultsfromsimulations andreal-imageryexperimentsperformedatANPL


where

where isthe6DOFcameraposeattimek

10

Notations

where


11

Assumptions

• Knownimagecorrespondencesforlandmarks&target

• White-GaussianNoises

• Markovprocess:Modelsdependonlyonthecurrentstateandpreviousstate

• Targetisrepresentedbyasinglelandmark

• Priorinformationonfirstcameraposeandtargetstate


12

Problem Formulation : BA and Target Tracking


13


• Jointprobabilitydistributionfunction(pdf)

• Maximumaposteriori(MAP)estimate:

MotionModel MeasurementModel

PriorInformation


14




PriorInformation


15

Measurement Model : Pinhole Camera

𝑝𝑟𝑜𝑗 𝑥, 𝑙 ≐ 𝐾 𝑅 𝑡 𝑙• DefiningtheProjectionOperator :

(u,v)

l

[R.I.Hartley,2004]

• ObservationModel: where

𝑝 𝑧|𝑥, 𝑙 =1

|2𝜋𝛴3|� exp −

12 ‖𝑧 − 𝑝𝑟𝑜𝑗 𝑥, 𝑙 ‖:;

<• MeasurementLikelihood:

Re-projectionerror

actual predicted


16




PriorInformation


17

Motion Model : Constant Velocity

• Targetstate:

• StatePropagation:

TransitionMatrix ProcessnoiseJacobian

• ConstantVelocity:

where

• ProbabilisticRepresentation:

and


18



• Pdfcanalsoberepresentedbygraphicalmodels:FactorGraph


• Verticesrepresentthevariables• Nodesrepresentconstrainsbetweenvariables,alsoknownasfactors

Allowsforcomputationallyefficientprobabilisticinference

Describesafactorizationofajointpdfintermsofprocessandmeasurementmodels

19

Factor Graph


20

Factor Graph : BA and Target Tracking

• Jointpdf :

• Maximumaposteriori(MAP)estimate:Involvesreconstructionofthe3Dstructure


• On-line3Dstructurereconstructionisofnointerest:

Marginalization

ComputationallyExpensiveProcess!

Increasesthecomputationalcomplexityoftheproblem

21

Computational Efficiency

• PerformedIncrementallyasnewsurroundingfeaturesareobserved

• ForBAandtargettrackingandwithNframesobservingMlandmarks:

LightBundleAdjustment(LBA)

:12Nelements

12N+3Melementstooptimize(6N– Camera,6N– Target,3M- Landmarks)


LessVariablesinvolved!(NoneedtocalculatefullBAfirst)

22

Incremental Light Bundle Adjustment (iLBA) – [Indelman et al., 2015]

[indelman etal.,2013]

• Allowstoalgebraicallyeliminatethelandmarksfromtheoptimization

• Intuition :3framesfromwhichthesamelandmarkisobservedarerelatedbygeometricalconstraints:Multiviewconstraints


2viewconstraints

3viewconstraint

23


• 2viewconstraints:epipolar geometry

• 3viewconstraints:relatesbetweenthescalesofand

[indelman etal.,2013]


2viewfactor

3viewfactor

24


BA LBA


BA LBA

25

… With target tracking

Thetargetistheonlyre-constructed3Dpoint!


26

Up till now

• Lessvariablestooptimize:12N+3M12NNframesMlandmarks

• Wesolve:

• How?


• Approaches:Gauss-Newton,Levenberg-Marquardt,…

27

Logismonotonic(samemax/min)

LBA and Target Tracking

• Recall:

Motionmodel Observationmodel 2v/3vconstraints(LBA)

• FindtheMAP:

• Equivalenttominimizing:


28

Optimization

RecipeforGauss-Newton:

• Linearizecostfunction

• Re-arange RHSsuchthat

• Solvefor

• Updatelinearizationpoint

• Repeatuntilconvergence

Note:- A containstheJacobiansofallthemeasurements

withrespecttothevariables- Aislarge!

- A issparse!

A

Jacobianmatrix

Picturesfrom[Dellaert etal.,2006]


29

Optimization

RecipeforGauss-Newton:

• Linearizecostfunction

• Re-arange RHSsuchthat

• Solvefor

• Updatelinearizationpoint

• Repeatuntilconvergence

Needtosolve

Expensiveprocess!

ATA

Informationmatrix

Picturesfrom[Dellaert etal.,2006]


30

Optimization

• Twoissues:

1. Naïveapproachisexpensive

2. The Entireprocessneedstobeperformedfromscratch eachtimeanewvariable/measurementisaddedtotheproblem

- SquareRootSAM(Dellaert etal.,2006)- IncrementalSAM- iSAM (Kaess etal.,2008)- iSAM2(Kaess etal.,2012)

• RecentlyDevelopedTechniques:

1. Exploitssparsity oftheinvolvedmatricestosimplifyrecovery

2. Usesgraphicalmodels toperformIncrementaloptimization :Calculationsfrompreviousstepscanbereused


31

Incremental Smoothing and Mapping (iSAM)

- Identifywhichvariablesareinvolvedinnewfactors- Rmatrixcanbeupdated,notrecalculated

x1 x2 x3 l

A

1. Exploitingmatrixsparsity

2. Usinggraphicalmodelstoallowforincrementaloptimization

• Factorization:QR(A),Cholesky (ATA)

QR: RSquare-rootmatrix


32

Up till now

Let’sseesomeresults!

• Onebigoptimizationprocessincludingcameraandtargetstates

• IntegratedtargettrackingintoiLBA framework:- Involveslessvariables(structure-less)- Performsincrementalinferenceovergraphical

models


33

Scenario


34

Experiments and Simulations

1. StatisticalSimulation:- ShortScenario

2. CaseStudy:- LargeScaleScenario

3. Real-ImageryExperiments


35

1. Statistical Simulation

• 45runMonte-Carlostudy• ShortScenario:52frames,160seconds• 2LoopClosures


36

• Largescenario:240frames,14.5km• ~25300observedLandmarks,~10LoopClosures

2. Large Scale Scenario Simulation


37

3. Real-Imagery Experiments• AerialScenario:Downwardfacingcameraobservingadynamictargetontheground• GroundTruthfrom6DoFopticaltrackingsystem

• Datasetspubliclyavailableat:http://vindelman.net.technion.ac.il/software/


38

3. Real-Imagery Experiments


ANPL1

- Circularrecurrentpath- Synchronizedmovements- Frequentloopcloser- Targetalwaysinsight

39

• 2differentdatasets



40

• 2differentdatasets


ANPL2

- Morecomplexpath- Unsynchronizedmovements- Targetnotalwaysinsight


41

ANPL 1


42

Real-Imagery results summary

Target Rel.Error[m] CameraRel.Error [m]

Mean Max Mean Max

ANPL1 0.06 0.19 0.01 0.09

ANPL2 0.01 0.42 0.01 0.23

ProcessingTime [sec]

Mean Total

ANPL1BA 5.6 447.8

LBA 2.2 177.1

ANPL2BA 3.1 222.9

LBA 1.9 139.4


Contributions

FutureWork

• Problemextensiontomulti-robot/multi-targetcasesChallenge:dataassociation

43

Conclusions / Future Work

• Anefficientmethodforvision-basedego-motionandtargettrajectoryestimationTargettrackingproblemisintegratedintotheiLBA framework

• Simulations/Testsshow:- ConsiderablegainincomputationaleffortscomparedtoBA- Similarlevelsofaccuracyforbothmethods

• Publiclyavailabledatasetsonline


44

THANKYOU

Vision-based Dynamic Target Trajectory and Ego-motion ...

Documents

Transcript of Vision-based Dynamic Target Trajectory and Ego-motion ...