Post on 19-May-2022
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
àWhyMotionEstimation?
SpaceExplorationSub-marineexploration
AutonomousDriving IndoorOperation
- AutonomousNavigation
Virtual/AugmentedReality
- Others
PointingDevices
3
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
àWhyTargetTracking?Surveillance Military
Robot– Humaninteraction
4
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
Scenario
- Unknownenvironment- Nopriorinformationaboutplatform’strajectory- Nopriorinformationabouttarget’strajectory
althoughmotionmodelisassumed
5
Useofonboardsensors:MonocularCamera
- Interestedinon-lineoperation- NoGlobalPositioningSystem
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
Scenario6
Efficientlyandsimultaneouslyestimateego-motionandtargettrajectory
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
Bundle Adjustment (BA) or Simultaneous Localization and Mapping (SLAM)
7
StaticLandmarks
DynamicTarget
SLAM + Detection and Tracking of Moving Object (DATMO)
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
9
Contributions
• PresentEgo-motionestimationandTargettrackingasacombinedoptimizationproblem
• Integratetargettrackingintoefficient“structure-less”BAframework:Use IncrementalLightBundleAdjustment(iLBA [Indelman etal.,2015])to:
- ImprovecomputationalefficiencycomparedtoBA- Incrementaloptimization:Re-usecalculationsfromprevioussteps
• Showresultsfromsimulations andreal-imageryexperimentsperformedatANPL
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
where
where isthe6DOFcameraposeattimek
10
Notations
where
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
11
Assumptions
• Knownimagecorrespondencesforlandmarks&target
• White-GaussianNoises
• Markovprocess:Modelsdependonlyonthecurrentstateandpreviousstate
• Targetisrepresentedbyasinglelandmark
• Priorinformationonfirstcameraposeandtargetstate
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
12
Problem Formulation : BA and Target Tracking
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
13
Problem Formulation : BA and Target Tracking
• Jointprobabilitydistributionfunction(pdf)
• Maximumaposteriori(MAP)estimate:
MotionModel MeasurementModel
PriorInformation
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
14
Problem Formulation : BA and Target Tracking
• Jointprobabilitydistributionfunction(pdf)
MotionModel MeasurementModel
PriorInformation
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
16
Problem Formulation : BA and Target Tracking
• Jointprobabilitydistributionfunction(pdf)
MotionModel MeasurementModel
PriorInformation
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
17
Motion Model : Constant Velocity
• Targetstate:
• StatePropagation:
TransitionMatrix ProcessnoiseJacobian
• ConstantVelocity:
where
• ProbabilisticRepresentation:
and
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
18
Problem Formulation : BA and Target Tracking
• Jointprobabilitydistributionfunction(pdf)
• Pdfcanalsoberepresentedbygraphicalmodels:FactorGraph
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
• Verticesrepresentthevariables• Nodesrepresentconstrainsbetweenvariables,alsoknownasfactors
Allowsforcomputationallyefficientprobabilisticinference
Describesafactorizationofajointpdfintermsofprocessandmeasurementmodels
19
Factor Graph
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
20
Factor Graph : BA and Target Tracking
• Jointpdf :
• Maximumaposteriori(MAP)estimate:Involvesreconstructionofthe3Dstructure
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
• On-line3Dstructurereconstructionisofnointerest:
Marginalization
ComputationallyExpensiveProcess!
Increasesthecomputationalcomplexityoftheproblem
21
Computational Efficiency
• PerformedIncrementallyasnewsurroundingfeaturesareobserved
• ForBAandtargettrackingandwithNframesobservingMlandmarks:
LightBundleAdjustment(LBA)
:12Nelements
12N+3Melementstooptimize(6N– Camera,6N– Target,3M- Landmarks)
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
LessVariablesinvolved!(NoneedtocalculatefullBAfirst)
22
Incremental Light Bundle Adjustment (iLBA) – [Indelman et al., 2015]
[indelman etal.,2013]
• Allowstoalgebraicallyeliminatethelandmarksfromtheoptimization
• Intuition :3framesfromwhichthesamelandmarkisobservedarerelatedbygeometricalconstraints:Multiviewconstraints
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
2viewconstraints
3viewconstraint
23
Incremental Light Bundle Adjustment (iLBA) – [Indelman et al., 2015]
• 2viewconstraints:epipolar geometry
• 3viewconstraints:relatesbetweenthescalesofand
[indelman etal.,2013]
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
2viewfactor
3viewfactor
24
Incremental Light Bundle Adjustment (iLBA) – [Indelman et al., 2015]
BA LBA
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
BA LBA
25
… With target tracking
Thetargetistheonlyre-constructed3Dpoint!
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
26
Up till now
• Lessvariablestooptimize:12N+3M12NNframesMlandmarks
• Wesolve:
• How?
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
• Approaches:Gauss-Newton,Levenberg-Marquardt,…
27
Logismonotonic(samemax/min)
LBA and Target Tracking
• Recall:
Motionmodel Observationmodel 2v/3vconstraints(LBA)
• FindtheMAP:
• Equivalenttominimizing:
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
28
Optimization
RecipeforGauss-Newton:
• Linearizecostfunction
• Re-arange RHSsuchthat
• Solvefor
• Updatelinearizationpoint
• Repeatuntilconvergence
Note:- A containstheJacobiansofallthemeasurements
withrespecttothevariables- Aislarge!
- A issparse!
A
Jacobianmatrix
Picturesfrom[Dellaert etal.,2006]
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
29
Optimization
RecipeforGauss-Newton:
• Linearizecostfunction
• Re-arange RHSsuchthat
• Solvefor
• Updatelinearizationpoint
• Repeatuntilconvergence
Needtosolve
Expensiveprocess!
ATA
Informationmatrix
Picturesfrom[Dellaert etal.,2006]
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
32
Up till now
Let’sseesomeresults!
• Onebigoptimizationprocessincludingcameraandtargetstates
• IntegratedtargettrackingintoiLBA framework:- Involveslessvariables(structure-less)- Performsincrementalinferenceovergraphical
models
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
33
Scenario
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
34
Experiments and Simulations
1. StatisticalSimulation:- ShortScenario
2. CaseStudy:- LargeScaleScenario
3. Real-ImageryExperiments
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
35
1. Statistical Simulation
• 45runMonte-Carlostudy• ShortScenario:52frames,160seconds• 2LoopClosures
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
36
• Largescenario:240frames,14.5km• ~25300observedLandmarks,~10LoopClosures
2. Large Scale Scenario Simulation
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
37
3. Real-Imagery Experiments• AerialScenario:Downwardfacingcameraobservingadynamictargetontheground• GroundTruthfrom6DoFopticaltrackingsystem
• Datasetspubliclyavailableat:http://vindelman.net.technion.ac.il/software/
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
38
3. Real-Imagery Experiments
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
ANPL1
- Circularrecurrentpath- Synchronizedmovements- Frequentloopcloser- Targetalwaysinsight
39
• 2differentdatasets
3. Real-Imagery Experiments
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
40
• 2differentdatasets
3. Real-Imagery Experiments
ANPL2
- Morecomplexpath- Unsynchronizedmovements- Targetnotalwaysinsight
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
41
ANPL 1
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
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
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
Contributions
FutureWork
• Problemextensiontomulti-robot/multi-targetcasesChallenge:dataassociation
43
Conclusions / Future Work
• Anefficientmethodforvision-basedego-motionandtargettrajectoryestimationTargettrackingproblemisintegratedintotheiLBA framework
• Simulations/Testsshow:- ConsiderablegainincomputationaleffortscomparedtoBA- Similarlevelsofaccuracyforbothmethods
• Publiclyavailabledatasetsonline
M.Chojnacki – Vision-basedTargetTrajectoryandEgo-motionEstimationusingiLBA – Seminar,Dec.2016
44
THANKYOU