Comparison of stereovision odometry approaches

Niko SuenderhaufChemnitz University of TechnologyGermany

Kurt KonolidgeSRI InternationalUSA

Thomas Lemaire, Simon LacroixRobotics and AI groupLAAS/CNRS, Toulouse

(at Laas 10/04 03/05)

(at Laas (08/04 03/05)

On the importance of localization

“Reach that goal”, “Map this area”… Missions are defined in terms of localization

Environment models are requiredSpatial consistency ensured by localization

Safe execution of the planned trajectoriesRobust control ensured by localization

“If you are not localized, you are lost !”

Outline

• Principle of stereovision odometry

• “Dead reckoning” approach

• More global approaches

• Conclusions

2. Pixels selection3. Pixels tracking

1. Stereovision

4.Stereovision

5. Motionestimation

Principle of stereovision odometry

Principle of stereovision odometry

• A lot of contributions now in the robotics literature– [Mallet-Lacroix-2000]– [Olson-Matthies-2000] - cf Matthies in the 80’s– [Corke-2004]– …

+ Related approaches– “scan matching” approaches - without image feature associations (e.g.

[Zhang-1992])– [Kim-ICRA-2005] - without stereo correspondences

• Three functionalities involved– Stereovision (sparse or dense)– Image feature association– Pose estimation

Feature tracking or feature matching ?

• Feature tracking Close images (high spatial rate) Aiding sensor (to focus the search) No feature selection necessarily required

• Feature matching Feature extraction / selection (or ?) A bit more time Work for almost any motion - no estimate necessary

Feature matching• Features : Harris precise detector [Schmid-ICCV-1998]• Interest point matching [Jung-ICCV-2001]

Feature matching• Features : Harris precise detector [Schmid-ICCV-1998]• Interest point matching [Jung-ICCV-2001]

Detected points Matched points An other example

Feature matching• Features : Harris precise detector [Schmid-ICCV-1998]• Interest point matching [Jung-ICCV-2001]

1.5 scale change 3.0 scale change

Error models

• Relation between correlation curve and d :

• Std dev. on disparities (here with ZNCC along epipolar)

1. On stereovision : empical analysis (cf [Matthies-1992])

2xd

x dx α

α=⇒=

)( cfd Error model :

Gaussian distributionCorrelation surface

Error models

2. On interest point matching : “gaussian fitting” model

Correlation surface locally computed around the matches (ZNCC score)

Validity of such a model ?Don’t we miss a proportional factor ?

Outline

• Principle of stereovision odometry– Feature matching– Error models

• “Dead reckoning” approach

• More global approaches

• Conclusions

Dead reckoning approach

• Relative t+1 / t poses computed with “constrained” least square minimisation (e.g. [Haralick-1989])

• Simple iterative outlier rejection algorithm (no RANSAC required)

Fairly good precision (up to 1% on 100m trajectories)

QuickTime™ et undécompresseur MPEG-4 vidéo

sont requis pour visionner cette image.

Dead reckoning error

• Propagating the uncertainty of 3D matching points set to optimal motion estimate [Haralick-1994]

- 3D matching points set

- Optimal motion estimate

- Cost function

€

J( ˆ u , ˆ Q ) = (X 'n −R( ˆ Θ , ˆ Φ , ˆ Ψ )Xn − ˆ t T )2

n=1

N

∑

]'',[ˆNN XXXXQQQ KK 11=Δ+=

),̂,̂,̂ˆ,ˆ,ˆ(ˆzyxuuu tttΨΦΘ=Δ+=

• Covariance of the random perturbation Δu : propagation using Taylor series expansion of the Jacobian of the cost function around Qu ˆ,ˆ

Outline

• Principle of stereovision odometry– Feature matching– Error models

• “Dead reckoning” approach

• More global approaches

• Conclusions

Bundle adjustment approach

• Classic way to solve the “structure from motion” problem in computer vision

• Non linear-minimization provides a MLE (up to a scale parameter)

• Can also optimize camera parameters (11 d.o.f. in Pi)

• n points, m poses : 3n + 6m parameters… Better have good initial estimates !

Sparse bundle adjustment [Hartley-2004]

€

Pi ,X i

min ωijd(PiX j , x ij )2

i, j

∑

€

X j

€

x ij = PiX j

: 3D points

: image coordinates

Sparse bundle adjustment with stereo

• “Simply” add second image pixels/poses in the function to minimize

• Naïve outlier rejection procedure costly (better use RANSAC ?)

• Various possibilities :– Used in a “dead reckoning” way– Used on a fixed size of images (or within a given distance) : “sliding

window approach”– Global optimization : “Full SBA”

SBA with stereo : indoor data set

Full SBA« local » SBA’s

SBA with stereo : outdoor data set

D-CP-GPSFull SBA« local » SBA’s

SBA with stereo : conclusions

• Full SBA simply not tractable (batch, required 4.5 min CPU time on the outdoor data set)

• Sliding window SBA seems better than dead-reckoning approach– Nb of images of 3-4 seems enough

EKF-SLAM approach

– Landmark detection– Relative observations (measures)

• Of the landmark positions• Of the robot motions

– Observation associations– Refinement of the landmark and robot positions

General SLAM operations

Vision : interest points

StereovisionVisual motion estimation / INS / Odo

Interest points matching Extended Kalman filter

“Stereo-based” SLAM operations

« Local memory » SLAM : forget landmarks that disappear• Can be run in « real time »• Can incorporate any aiding sensor• Various « forget strategies » can be defined

Local EKF-SLAM approach : data set

QuickTime™ et undécompresseur MPEG-4 vidéo

sont requis pour visionner cette image.

Along a 60m loop trajectory :• 100 stereo pairs• Looking inwards

By the way, between images 31 and 32 :

Local EKF-SLAM approach : results

Local EKF-SLAM approach : results

(Full EKF-SLAM approach : results)

landmark uncertainty ellipses (x5)

(Full EKF-SLAM approach : results)

Frame 1/100

Reference

Reference

Std. Dev.

VME

result

VME

Abs.error

SLAM

result

SLAM

Std. Dev.

SLAM

Abs. error

Θ 0.52° 0.31° 2.75° 2.23° 0.88° 0.98° 0.36 °

Φ 0.36° 0.25° -0.11° 0.47° 0.72° 0.74° 0.36 °

-0.14° 0.16° 1.89° 2.03° 1.24° 1.84° 1.38°

tx -0.012m

0.010m

0.057m0.069

m-

0.077m0.069

m0.065

m

ty -0.243m

0.019m

-1.018m0.775

m-

0.284m0.064

m0.041

m

tz 0.019m0.015

m0.144m

0.125m

0.018m0.019

m0.001

m

(((( Full EKF-SLAM approach : results ))))

QuickTime™ et undécompresseur Cinepak

sont requis pour visionner cette image.

Outline

• Principle of stereovision odometry– Feature matching– Error models

• “Dead reckoning” approach

• More global approaches– SBA-based approach– EKF-SLAM approach

• Conclusions ?

Conclusions

• A vast number of “parameters” to check/assess– Algorithmic parameters :

• Kind of matching algorithm (stereo and motion matches)• Feature definition and selection • Estimation

– Dead reckoning– SBA approaches– SLAM approaches

– System parameters :• Image size• Focal length• Stereovision baseline and height• Bench orientation (forward, sidewards, downwards)

Panoramic cameras !!!(not even stereo ? cf “visual-SLAM” recent results, “view-based” localisation…

The Journal of Field Robotics seeks to promote rapid dissemination of important research results in robotics for unstructured and dynamic environments.  Articles describing robotics research with applications to the environment, construction, forestry, agriculture, ,mining, subsea, intelligent highways, search and rescue, military, and space (orbital and planetary) are encouraged. Articles in sensing, sensors, mechanical design, computing architectures, communication, planning, learning, and control, applied to field applications are encouraged.

The first issue is expected to be available in January 2006.

Further Details: http://www.ri.cmu.edu/~jfr

Journal of Field RoboticsEditor-In-ChiefSanjiv Singh, Carnegie Mellon Editorial BoardRobert Ambrose, NASA JSC Greg Baiden, Laurentian Univ. Martin Buehler, Boston DynamicsRaja Chatila, LAASPeter Corke, CSIRO Eric Feron, MITErnie Hall, Univ. of CincinnatiAlonzo Kelly, CMULarry Matthies, NASA JPLEduardo Nebot, Univ. of SydneySimon LaCroix, LAAS Annibal Ollero, Univ. of SevilleVincent Rigaud, IFREMER

David Wettergreen, CMURon Arkin, Georgia Tech,Alberto Broggi, Univ. of Parma Aarne Halme, HUT Peter Lawrence, Univ. of British Columbia David Nister, Univ. of Kentucky John Reid, John Deere Mirek Skibinewski, Purdue James Trevelyan, Univ of Western Australia Tony Stentz, CMUBrian Wilcox, NASA JPLKazuya Yoshida, Tohoku Univ.

Jonathan Roberts, CSIRO