Ryan Eustice1,2, Hanumant Singh1, John Leonard2, Matthew Walter2, and Robert Ballard3

1Woods Hole Oceanographic Institution, 2Massachusetts Institute of Technology, 3University of Rhode Island

Fig. 2. The recovered XY trajectory and covariance bounds for the RMS Titanic. (left) Final pose constraint­network associated with using 866 images to provide 3494 camera constraints; 3­sigma bounds are shown. (right) A zoomed view illustrating the consistency of the data association bounds generated by our algorithm. Note that for this plot the 3­sigma bounds have been inflated by a factor of 30 for visualization. In this plot we have: 1) the initial covariance bounds associated with pose insertion (red), 2) the marginal covariance bounds based upon constant­time Kalman updates (gray), and 3) the actual marginal covariance bounds obtained by matrix inversion (green). Note that all of the estimated bounds (gray) were verified to be consistent with respect to the actual marginal covariance (green) by performing Cholesky decomposition on their difference to establish positive definiteness. The reason why some of the estimated covariance bounds (gray) are tighter approximations than others to the actual  filter bounds (green) is because our algorithm only updates the bounds on a per re­observation basis. Hence, if the robot is suffciently well­localized when it re­observes an image, then the covariance bound of the corresponding delayed­state improves. 

AcknowledgementsThis work was supported in part by the CenSSIS ERC of the National Science Foundation under grant EEC­9986821.

ConclusionIn conclusion, we have presented a novel algorithm for extracting consistent covariance bounds useful for data association in SLAM information filters.  We showed that our method provides a conservative approximation useful for real­world tasks such a image link hypothesis and constrained correspondence searches.  The method's complexity scales asymptotically linear with map size as measured by solving for the robot's covariance­column coupled with fixed state­size Kalman updates for re­observed map elements.  Our results were presented within the context of an actual robotic mapping survey of the RMS Titanic embodying several challenging SLAM research tasks including large­area scalable mapping, 6­DOF, an unstructured underwater environment, and visual perception.

Contact: Ryan EusticeWoods Hole Oceanographic Institution, Woods Hole, MA 02543Ph: 508.289.3269Email: ryan@whoi.eduWeb: http://www.whoi.edu/people/reustice

AbstractThis poster addresses the problem of precision navigation and mapping using low­overlap, high resolution image sequences obtained by autonomous undersea vehicles; for this purpose we adopt a view­based simultaneous localization and mapping (SLAM) approach.  From a “robotics science” perspective, our primary contribution consists of an efficient algorithm for extracting consistent covariance bounds from SLAM information filters, which is important for data association (i.e. the problem of establishing correspondence to measurements).  From a “robotics systems” perspective, we demonstrate automatic, 6­DOF, single camera, visually­augmented navigation (VAN) processing of a sequence of 866 digital­still images of the RMS Titanic (Fig. 1), for a mission with a vehicle path length over 3 km long and over 3100 m2 of mapped area.

Covariance Recovery MethodOur novel approach for efficient consistent covariance recovery…

Problem: naïvely recovering the covariance matrix,      , from the information matrix,     , using matrix inversion results in cubic complexity.

Require: what do we need for ML data association?

Approach: maintain consistent estimates of the covariance block diagonal along with the robot's cross­covariance.

Mechanism: solve a sparse linear system of equations to additionally obtain the robot covariance column during state recovery.

Initial covariance bound: inserting a state into our view­based map.

Data association: a conservative, valid, postive­semidefinite, joint­covariance approximation can be obtained as:

Bound Update: tighten the bound,                            , on a per reobservation basis using our conservative marginal, mesurement z, and a constant­time Kalman update.*

*Note that by conceptually performing the marginalization step above before computing the Kalman update, we have avoided any inconsistency issues associated with only storing the marginal bounds,      , and not representing the intra­map correlations.  This ensures that that our update step will result in a consistent marginal bound for data assocation that will improve over time as we reobserve map elements.

Visually Navigating the RMS Titanic with SLAM Information Filters

Fig. 1.  Mapping results from a summer of 2004 ROV survey of the RMS Titanic. (top) An illustration of the Markov network associated with our view­based SLAM approach.  (middle) XY plot comparing the raw dead­reckoned navigation data (brown), ship­board ultra­short baseline tracking (gray), and reconstructed survey trajectory from a vision­based 6­DOF SLAM information filter (red). (bottom) A photomosaic of the RMS Titanic constructed from over 700 digital­still images.  Note that this photomosaic is presented for visualization purposes only as a representation of the data that serves as input to our algorithm.

Survey started here at midships and then proceeded towards the stern.

After reaching the stern, the vehicle was piloted back towards the starting point with the camera turned off.  During this return trip proprioceptive navigation measurements dropped out for a period of time (causing the discontintuty in the DR trajectory above). Upon reaching the starting point, the camera was turned back on and the survey completed finishing with the bow.

Consistencyrobot marginal 

delayed­state marginalcross­covariance

nav nav nav nav

cameracamera

cameracamera

initial bound:@ state augmentation we have: since        can only decrease,          will provide a conservative bound for all time:

filter bank

true marginal

Kalman gain

accessing statistics

Consistent Approx.

our techniquematrix inversionMarkov Blanket

Fig. 3.  Comparison of different covariance recovery methods within the context of two­view image registration.  The epipolar lines (green) are instantiated between the two views based upon our pose prior with the search bands corresponding to 6­sigma bounds.  Shown in red is the Markov blanket covariance recovery technique based upon conditional covariances, in yellow is the actual bound based upon matrix inversion, and in blue is the conservative bound based upon our novel covariance recovery technique. (top) A pair of images for which the Markov blanet covariance recovery method fails.  Note that the true correspondence does not lie within the red search ellipse, but does within the yellow and blue ellipses.  Hence, image correspondence establishment would fail for the Markov blanket approximation.(bottom) A pair of images for which all three methods produce equivalent results.  This highlights the unpredictibility in the Markov blanket approximation.

ResultsFig. 4.  (top) VAN's output is the 6­DOF vehicle trajectory sampled at image acquisition.  (bottom) As a sanity check on VAN's pose estimates, we can pairwise triangulate the scene structure based upon the estimated camera poses and image­to­image correspondences.

VAN estimated vehicle poses

Pairwise triangulated 3D point cloud with trajectory overlaid

Textured mapped surface

extracted from

conservative marginal

Delaunaymesh