Sliding window filter with application to planetary landing

We are concerned with improving the range resolution of stereo vision for entry, descent, and landing (EDL) missions to Mars and other planetary bodies. The goal is to create accurate and precise three‐dimensional planetary surface‐structure estimates by filtering sequences of stereo images taken from an autonomous landing vehicle. We describe a sliding window filter (SWF) approach based on delayed state marginalization. The SWF can run in constant time, yet still achieve experimental results close to those of the bundle adjustment solution. This technique can scale from the offline batch least‐squares solution to fast online incremental solutions. For instance, if the window encompasses all time, the solution is equivalent to full bundle adjustment; if only one time step is maintained, the solution matches the extended Kalman filter; if poses and landmarks are slowly marginalized out over time such that the state vector ceases to grow, then the filter becomes constant time, like visual odometry. Within the constant time regime, the sliding window approach demonstrates convergence properties that are close to those of the full batch solution and strictly superior to visual odometry. Experiments with real data show that ground structure estimates follow the expected convergence pattern that is predicted by theory. These experiments indicate the effectiveness of filtering long‐range stereo for EDL. © 2010 Wiley Periodicals, Inc.

[1]  G. Saridis.  Parameter estimation: Principles and problems , 1983, Proceedings of the IEEE.

[2]  Evangelos E. Milios,et al.  Globally Consistent Range Scan Alignment for Environment Mapping , 1997, Auton. Robots.

[3]  K. F. Gauss,et al.  Theoria combinationis observationum erroribus minimis obnoxiae , 1823 .

[4]  Mark A. Paskin,et al.  Thin Junction Tree Filters for Simultaneous Localization and Mapping , 2002, IJCAI.

[5]  David W. Murray,et al.  A unifying framework for structure and motion recovery from image sequences , 1995, Proceedings of IEEE International Conference on Computer Vision.

[6]  Edwin Olson,et al.  Fast iterative alignment of pose graphs with poor initial estimates , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[7]  Andrew J. Davison,et al.  Real-time simultaneous localisation and mapping with a single camera , 2003, Proceedings Ninth IEEE International Conference on Computer Vision.

[8]  Tom Duckett,et al.  A Multigrid Approach for Acceleration Relaxation-Based SLAM , 2003, AMS.

[9]  Wolfram Burgard,et al.  A Tree Parameterization for Efficiently Computing Maximum Likelihood Maps using Gradient Descent , 2007, Robotics: Science and Systems.

[10]  Stephen A. Cook,et al.  The complexity of theorem-proving procedures , 1971, STOC.

[11]  Andrew W. Fitzgibbon,et al.  Bundle Adjustment - A Modern Synthesis , 1999, Workshop on Vision Algorithms.

[12]  Simon J. Julier.  The stability of covariance inflation methods for SLAM , 2003, Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453).

[13]  Andrew Calway,et al.  Real-Time Camera Tracking Using a Particle Filter , 2005, BMVC.

[14]  S. F. Schmidt,et al.  The Kalman filter - Its recognition and development for aerospace applications , 1981 .

[15]  Larry H. Matthies,et al.  Error Modelling in Stereo Navigation , 1986, FJCC.

[16]  Udo Frese.  A Proof for the Approximate Sparsity of SLAM Information Matrices , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[17]  Bernhard P. Wrobel,et al.  Multiple View Geometry in Computer Vision , 2001 .

[18]  David Nister,et al.  Bundle Adjustment Rules , 2006 .

[19]  Frank Dellaert,et al.  Incremental smoothing and mapping , 2008 .

[20]  A. McEwen,et al.  Mars Reconnaissance Orbiter's High Resolution Imaging Science Experiment (HiRISE) , 2007 .

[21]  G. Bierman.  Factorization methods for discrete sequential estimation , 1977 .

[22]  W. Burgard,et al.  Markov Localization for Mobile Robots in Dynamic Environments , 1999, J. Artif. Intell. Res..

[23]  Bradley M. Bell,et al.  The Iterated Kalman Smoother as a Gauss-Newton Method , 1994, SIAM J. Optim..

[24]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.

[25]  Peter J. Rousseeuw,et al.  Robust regression and outlier detection , 1987 .

[26]  Zhengyou Zhang,et al.  Parameter estimation techniques: a tutorial with application to conic fitting , 1997, Image Vis. Comput..

[27]  J. M. M. Montiel,et al.  The SPmap: a probabilistic framework for simultaneous localization and map building , 1999, IEEE Trans. Robotics Autom..

[28]  Simon J. Godsill,et al.  On sequential Monte Carlo sampling methods for Bayesian filtering , 2000, Stat. Comput..

[29]  Peter C. Cheeseman,et al.  Estimating uncertain spatial relationships in robotics , 1986, Proceedings. 1987 IEEE International Conference on Robotics and Automation.

[30]  James R. Bergen,et al.  Visual odometry , 2004, Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2004. CVPR 2004..

[31]  Jeffrey K. Uhlmann,et al.  Using covariance intersection for SLAM , 2007, Robotics Auton. Syst..

[32]  Martial Hebert,et al.  Bearings-only localization and mapping , 2005 .

[33]  Marc Pollefeys,et al.  Multiple view geometry , 2005 .

[34]  Eduardo Mario Nebot,et al.  Optimization of the simultaneous localization and map-building algorithm for real-time implementation , 2001, IEEE Trans. Robotics Autom..

[35]  Hugh F. Durrant-Whyte,et al.  Simultaneous Mapping and Localization with Sparse Extended Information Filters: Theory and Initial Results , 2004, WAFR.

[36]  Hugh Durrant-Whyte,et al.  SLAM Updates Require Constant Time , 2002 .

[37]  Christopher G. Harris,et al.  A Combined Corner and Edge Detector , 1988, Alvey Vision Conference.

[38]  C. N. Liu,et al.  Approximating discrete probability distributions with dependence trees , 1968, IEEE Trans. Inf. Theory.

[39]  Michel Dhome,et al.  Real Time Localization and 3D Reconstruction , 2006, 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR'06).

[40]  Edward M. Mikhail,et al.  Observations And Least Squares , 1983 .

[41]  Peter Cheeseman,et al.  On the Representation and Estimation of Spatial Uncertainty , 1986 .

[42]  John J. Leonard,et al.  Towards Constant-Time SLAM on an Autonomous Underwater Vehicle Using Synthetic Aperture Sonar , 2003, ISRR.

[43]  Andrew W. Fitzgibbon,et al.  Automatic Camera Recovery for Closed or Open Image Sequences , 1998, ECCV.

[44]  F. A. Seiler,et al.  Numerical Recipes in C: The Art of Scientific Computing , 1989 .

[45]  Gaurav S. Sukhatme,et al.  The Iterated Sigma Point Kalman Filter with Applications to Long Range Stereo , 2006, Robotics: Science and Systems.

[46]  Hans P. Moravec.  Obstacle avoidance and navigation in the real world by a seeing robot rover , 1980 .

[47]  Wolfram Burgard,et al.  Simultaneous Localisation and Mapping in Dynamic Environments (SLAMIDE) with Reversible Data Association , 2008 .

[48]  Y. Bar-Shalom.  Tracking and data association , 1988 .

[49]  Edward J. Krakiwsky,et al.  Geodesy, the concepts , 1982 .

[50]  Takeo Kanade,et al.  An Iterative Image Registration Technique with an Application to Stereo Vision , 1981, IJCAI.

[51]  Udo Frese.  Efficient 6-DOF SLAM with Treemap as a Generic Backend , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[52]  Paul Newman,et al.  Navigating, Recognizing and Describing Urban Spaces With Vision and Lasers , 2009, Int. J. Robotics Res..

[53]  Jonathan M. Garibaldi,et al.  Fast, unconstrained camera motion estimation from stereo without tracking and robust statistics , 2002, 7th International Conference on Control, Automation, Robotics and Vision, 2002. ICARCV 2002..

[54]  Gilbert Strang,et al.  Introduction to applied mathematics , 1988 .

[55]  Kurt Konolige,et al.  Frame-Frame Matching for Realtime Consistent Visual Mapping , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[56]  Kaichang Di,et al.  CAHVOR camera model and its photogrammetric conversion for planetary applications , 2004 .

[57]  Peter S. Maybeck,et al.  Stochastic Models, Estimation And Control , 2012 .

[58]  B. Tapley,et al.  Statistical Orbit Determination , 2004 .

[59]  Michael Bosse,et al.  An Atlas framework for scalable mapping , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[60]  Sebastian Thrun,et al.  FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges , 2003, IJCAI.

[61]  C. Ghilani,et al.  Adjustment Computations: Statistics and Least Squares in Surveying and GIS , 1987 .

[62]  Clark F. Olson,et al.  Stereo ego-motion improvements for robust rover navigation , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[63]  G. Stewart,et al.  Theory of the Combination of Observations Least Subject to Errors , 1987 .

[64]  David W. Murray,et al.  Improving the Agility of Keyframe-Based SLAM , 2008, ECCV.

[65]  Javier Civera,et al.  Unified Inverse Depth Parametrization for Monocular SLAM , 2006, Robotics: Science and Systems.

[66]  Stergios I. Roumeliotis,et al.  Vision‐aided inertial navigation for pin‐point landing using observations of mapped landmarks , 2007, J. Field Robotics.

[67]  Hanumant Singh,et al.  Visually Navigating the RMS Titanic with SLAM Information Filters , 2005, Robotics: Science and Systems.

[68]  John J. Leonard,et al.  A Computationally Efficient Method for Large-Scale Concurrent Mapping and Localization , 2000 .

[69]  Hugh F. Durrant-Whyte,et al.  Data association for mobile robot navigation: a graph theoretic approach , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[70]  Frederick R. Forst,et al.  On robust estimation of the location parameter , 1980 .

[71]  Frank Dellaert,et al.  Fast 3D pose estimation with out-of-sequence measurements , 2007, 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[72]  A.H. Haddad,et al.  Applied optimal estimation , 1976, Proceedings of the IEEE.

[73]  Rama Chellappa,et al.  Structure from Motion Using Sequential Monte Carlo Methods , 2004, International Journal of Computer Vision.

[74]  Paul Newman,et al.  On the Structure and Solution of the Simultaneous Localisation and Map Building Problem , 1999 .

[75]  Walter L. Smith.  Probability and Statistics , 1959, Nature.

[76]  Tom Drummond,et al.  Machine Learning for High-Speed Corner Detection , 2006, ECCV.

[77]  Eduardo Mario Nebot,et al.  Solving computational and memory requirements of feature-based simultaneous localization and mapping algorithms , 2003, IEEE Trans. Robotics Autom..

[78]  John E. Dennis,et al.  Numerical methods for unconstrained optimization and nonlinear equations , 1983, Prentice Hall series in computational mathematics.

[79]  Gabe Sibley.  Sliding Window Filters for SLAM , 2006 .

[80]  José A. Castellanos,et al.  Simultaneous Localization and Map Building , 1999 .

[81]  Stergios I. Roumeliotis,et al.  A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[82]  Philip F. Mclauchlan,et al.  The Variable State Dimension Filter applied to Surface-Based Structure from Motion , 1999 .

[83]  James M. Ortega,et al.  Iterative solution of nonlinear equations in several variables , 2014, Computer science and applied mathematics.

[84]  Frank Dellaert,et al.  Square Root SAM , 2005, Robotics: Science and Systems.

[85]  James J. Little,et al.  σMCL: Monte-Carlo Localization for Mobile Robots with Stereo Vision , 2005, Robotics: Science and Systems.

[86]  F. W. Cathey,et al.  The iterated Kalman filter update as a Gauss-Newton method , 1993, IEEE Trans. Autom. Control..

[87]  Ian D. Reid,et al.  Vast-scale Outdoor Navigation Using Adaptive Relative Bundle Adjustment , 2010, Int. J. Robotics Res..

[88]  J. J. Moré,et al.  Levenberg--Marquardt algorithm: implementation and theory , 1977 .

[89]  H. Sorenson,et al.  NONLINEAR FILTERING BY APPROXIMATION OF THE A POSTERIORI DENSITY , 1968 .