Xun (Sam) Zhou Multiple Autonomous Robotic Systems (MARS) Lab Dept. of Computer Science and Engineering University of Minnesota Algebraic Geometry in Computer Vision and Robotics

2 Introduction Geometric problems widely appear in computer vision/robotics –Visual Odometry –Map-based localization (image/laser scan) –Manipulators We need to solve systems of polynomial equations Stewart Mechanism

3 Outline Visual odometry with directional correspondence Motion-induced robot-to-robot extrinsic calibration Optimal motion strategies for leader-follower formations p C {F} {L} {F} g

4 Motivation Main challenge: data association Outlier rejection (RANSAC)  least-squares refinement Objective: efficient minimal solvers Min. No. points Minimize prob. of picking an outlier

5 Related Work Five points (10 solutions) –[Nister ’04] Compute null space of a 5x9 matrix Gauss elimination of a dense 10x20 matrix Solve a 10 th order polynomial  essential matrix Recover the camera pose from the essential matrix Three points and one direction (4 solutions) –[Fraundorfer et al. ’10] Similar to the 5-point algorithm w. fewer unknowns Solve a 4 th order polynomial  essential matrix –[Kalantari et al. ’11] Tangent half-angle formulae Singularity at 180 degree rotation Solve a 6 th order polynomial (2 spurious solutions) –Our algorithm Fast: coefficient of the 4 th order polynomial in closed form Solve for the camera pose directly

6 Problem Formulation Directional constraint 3 point matches {1} {2} 2-DOF in rotation 1-DOF in rotation 2-DOF in translation (scale is unobservable) Objective: determine

7 Determine 2-DOF in Rotation Parameterization of R: Compute

8 Problem reformulation Determine the Remaining 3-DOF Linear in System of polynomial equations in

9 Problem solution Eliminate Eliminate using Sylvester resultant Back-substitute to solve for Step 1 Step 2 Step 3 4 th order 4 solutions for Determine the Remaining 3-DOF

10 Simulation Results Under image and directional noise –Directional noise (deg): rotate around random axis –Report median errors Observations –Forward motion out performs sideway –Rotation estimate better than translation [Courtesy of O. Naroditsky, UPenn]

11 Simulation Results Comparison with the Five-point algorithm –Directional reference: image points at infinity 3p1 outperforms 5-point –Rotation estimation –Translation in forward motion –5 times faster (2.6 μs vs μs) 3p1: 3 plus 1 method 5p : five-point method [Courtesy of O. Naroditsky, UPenn]

12 Experimental Results Sample Images Setup –Single camera (640x480 pixels, 50 degree FOV) –Record an 825-frame outdoor sequence, total of 430 m trajectory –RANSAC: 200 hypotheses for each image pair 3p1 has 2 failures, while 5-point has 4 failures Fail to choose inlier set [Courtesy of O. Naroditsky, UPenn]

13 Outline Visual odometry with directional correspondence Motion-induced robot-to-robot extrinsic calibration Optimal motion strategies for leader-follower formation p C {F} {L} {F} g

14 Multi-robot tracking (MARS) Introduction Motivating applications –Cooperative SLAM –Multi-robot tracking –Formation flight Require global/relative robot pose Formation Flight (NASA) Satellite Formation Flight (NASA) Talisman L (BAE Systems) Multi-robot tracking (MARS)

15 Multi-robot tracking (MARS) Motivating applications –Cooperative SLAM –Multi-robot tracking –Formation flight Determine relative pose using –External references (e.g., GPS, map) Not always available –Ego motion and robot-to-robot measurements Distance and/or Bearing Requires solving systems of nonlinear (polynomial) equations Contributions –Identified 14 minimal problems using combinations of robot-to-robot measurements (distance and/or bearing) –Provided closed-form or efficient solutions Require global/relative robot pose Formation Flight (NASA) Talisman L (BAE Systems) Introduction

16 Problem Description {2} {1} d 12 b1b1 b2b2 Goal: Determine relative pose (p, C) for robots moving in 3D p C First meet at {1}, {2}, measure subset of {d 12, b 1, b 2 }

17 Problem Description {2} {1} d 12 b1b1 b2b2 2p42p4 1p31p3 {3} {4} d 34 b3b3 b4b4 Goal: Determine relative pose (p, C) for robots moving in 3D p C First meet at {1}, {2}, measure subset of {d 12, b 1, b 2 } Then move to {3}, {4}, measure subset of {d 34, b 3, b 4 }

18 Problem Description and Related Work {2} {1} d 12 b1b1 b2b2 2p42p4 1p31p3 {3} {4} d 34 b3b3 b4b4 1 p 2n-1 2 p 2n {2n} d 2n-1, 2n b 2n-1 b 2n {2n-1}... Goal: Determine relative pose (p, C) for robots moving in 3D p C First meet at {1}, {2}, measure subset of {d 12, b 1, b 2 } Then move to {3}, {4}, measure subset of {d 34, b 3, b 4 } Collect at least 6 scalar measurements for determining the 6-DOF relative pose Homogeneous (Minimal) 6 distances [Wampler ’96], [Lee & Shim ’03] [Trawny, Zhou, et al. RSS’09] Homogeneous (Overdetermined) Distance and/or bearing [Trawny, Zhou, et al. TRO’10] Stewart Mechanism

19 Homogeneous (Minimal) 6 distances [Wampler ’96], [Lee & Shim ’03] [Trawny, Zhou, et al. RSS’09] Homogeneous (Overdetermined) Distance and/or bearing [Trawny, Zhou, et al. TRO’10] Heterogeneous (Minimal) (e.g., ) Our focus Problem Description and Related Work {2} {1} b1b1 2p42p4 1p31p3 {3} {4} d 34 b4b4 1p51p5 2p62p6 {6} d 56 {5} Goal: Determine relative pose (p, C) for robots moving in 3D p C First meet at {1}, {2}, measure subset of {d 12, b 1, b 2 } Then move to {3}, {4}, measure subset of {d 34, b 3, b 4 } Collect at least 6 scalar measurements for determining the 6-DOF relative pose

20 Combinations of Inter-robot Measurements No. of eqns All possible combinations up to 6 time steps 7^6 =117,649 (overdetermined) problems! scalar  1 equation3D unit vector  2 equations

21 Only 14 Minimal Systems No. of eqns [IROS ’10] [ICRA ’11] [RSS ’09] Sys10 These are formulated as systems of polynomial equations.

22 Relative position known From the distance Solve for C from system of equations System 10: d 12 p {4 } C {1} 2p42p4 1p31p3 b1b1 {3} d 78 {2 } {7} {8 } 8 solutions solved by multiplication matrix 2p82p8 1p71p7 d 34...

23 Methods for Solving Polynomial Equations Elimination & back-substitution Multiplication (Action) matrix Original system Triangular system Multiplication Matrix Eigendecomp. m solutions Resultant Symbolic- Numerical method Groebner Basis

24 Multiplication Matrix of a Univariate Polynomial Monomials in the remainder of any polynomial divided by f

25 Extension to Multivariable Case

26 Solve System 10 by Multiplication Matrix Represent rotation by Cayley’s parameter Find the Multiplication matrix via Macaulay Resultant Quadratic in s Add a linear function: multiply with some monomials Arrange polynomials in matrix form: Eliminate Read off solutions from eigenvectors 8 basis monomials 27 extra monomials

27 Outline Visual odometry with directional correspondence Motion-induced robot-to-robot extrinsic calibration Optimal motion strategies for leader-follower formations p C {F} {L} {F} g

28 Optimal Motion Strateges for Leader-Follower Formations Vehicles often move in formation V formation flight [aerospaceweb.org] Platooning [tech-faq.com] X. S. Zhou, K. Zhou, S. I. Roumeliotis, Optimized Motion Strategies for Localization in Leader-Follower Formations, IROS (To appear)

29 Optimal Motion Strateges for Leader-Follower Formations Vehicles often move in formation to improve fuel efficiency Robot motion affects estimation accuracy Next-step optimal motion strategies Finding all critical points that satisfy the KKT optimality conditions {L} {F} In formation, relative pose unobservable distance, or bearing Uncertainty unbounded

30 Simulation Results: Range-only Leader moves on straight line Follower desired position Initial covariance Measurement noise MTF: maintaining the formation CRM: constrained random motion MME: active control strategy [Mariottini et al.] GBS: grid-based search RAM: our relaxed algebraic method Follower TrajectoryAverage over 50 Monte Carlo trials

31 Summary Algebraic geometric has wide range of applications Other projects I have also worked on –Multi-robot SLAM –Vision-aided inertial navigation Visual Odometry p C Motion-induced Extrinsic Calibration and more … {F} {L} Optimal Motion

Xun (Sam) Zhou Multiple Autonomous Robotic Systems (MARS) Lab Dept. of Computer Science and Engineering University of Minnesota Algebraic Geometry in Computer Vision and Robotics