xun (sam) zhou multiple autonomous robotic systems (mars) lab dept. of computer science and...
TRANSCRIPT
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