xun (sam) zhou multiple autonomous robotic systems (mars) lab dept. of computer science and...

Xun (Sam) Zhou

Multiple Autonomous Robotic Systems (MARS) Lab

Dept. of Computer Science and Engineering

University of Minnesota

Algebraic Geometry inComputer 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

pC

{F}

{L}{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 10th 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 4th order polynomial essential matrix

– [Kalantari et al. ’11] • Tangent half-angle formulae• Singularity at 180 degree rotation• Solve a 6th order polynomial (2 spurious solutions)

– Our algorithm• Fast: coefficient of the 4th 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 rotation2-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 solutionEliminate

Eliminate using Sylvester resultant

Back-substitute to solve for

Step 1

Step 2

Step 3

4th 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]

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

pC

{F}

{L}{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)

Talisman L (BAE Systems)

Introduction

16

Problem Description

{2}

{1}

d12

b1

b2

Goal: Determine relative pose (p, C) for robots moving in 3D

pC

• First meet at {1}, {2}, measure subset of {d12, b1, b2 }

17

Problem Description

{2}

{1}

d12

b1

b2

2p4

1p3

{3}

{4}

d34

b3

b4

Goal: Determine relative pose (p, C) for robots moving in 3D

pC

• First meet at {1}, {2}, measure subset of {d12, b1, b2 }

• Then move to {3}, {4}, measure subset of {d34, b3, b4 }

18

Problem Description and Related Work

{2}

{1}

d12

b1

b2

2p4

1p3

{3}

{4}

d34

b3

b4

1p2n-1

2p2n

{2n}

d2n-1, 2n

b2n-1

b2n

{2n-1}

...

Goal: Determine relative pose (p, C) for robots moving in 3D

pC

• First meet at {1}, {2}, measure subset of {d12, b1, b2 }

• Then move to {3}, {4}, measure subset of {d34, b3, b4 }

• 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}

b1

2p4

1p3

{3}

{4}

d34

b4

1p5

2p6

{6}

d56

{5}

Goal: Determine relative pose (p, C) for robots moving in 3D

pC

• First meet at {1}, {2}, measure subset of {d12, b1, b2 }

• Then move to {3}, {4}, measure subset of {d34, b3, b4 }

• Collect at least 6 scalar measurements for determining the 6-DOF relative pose

20

Combinations of Inter-robot Measurements

4

6

5

1

2

3

No. ofeqns

All possible combinations up to 6 time steps 7^6 =117,649 (overdetermined) problems!

scalar 1 equation 3D unit vector 2 equations

21

Only 14 Minimal Systems

4

6

5

1

2

3

No. ofeqns

[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:

d12

p

{4}

C

{1}

2p4

1p3

b1

{3}

d78

{2}

{7}

{8}

8 solutions solved by multiplication matrix

2p8

1p7

d34 ...

23

Methods for Solving Polynomial Equations

• Elimination & back-substitution

• Multiplication (Action) matrix

Original system Triangular system

MultiplicationMatrix

Eigendecomp.

m solutions

Resultant

Symbolic-Numerical

method

Groebner Basis

24

Multiplication Matrix of a Univariate Polynomial

Monomials in the remainderof 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

pC

{F}

{L}{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 2011. (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 trialsAverage 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

pC

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 inComputer Vision and Robotics