The Variable State Dimension Filter applied to Surface-Based Structure from Motion

The present work is a long-term solution to the problem of scene and motion reconstruction. Our goal is to develop algorithmic and statistical tools that combine data from multiple images and other sensors. We have previously presented specific applications of our Variable State Dimension Filter (VSDF)method in the areas of 3D reconstruction from image sequences [34, 36], vehicle navigation [33], image mosaicing [32], camera calibration [37]. We also envisage application to sensor fusion and active vision (e.g. tracking/servoing). The thrust of this work is to develop general, scalable tools that allow data of multiple types to be incorporated (for instance point and line features), different projection models to be used (e.g. projective/affine/perspective camera models), and information from different sensors to be integrated (for example cameras, range finders, odometry). Most current approaches to scene reconstruction are batch approaches that apply to a set of images processed offline. A natural and inevitable development will be towards online systems, wherein the results are computed as the data is acquired. This will required recursive algorithms that provide constant computation effort as each new image is added to the sequence. Thus a central feature of our VSDF algorithm is that it runs both in batch mode and recursive mode. The usual manner of running the VSDF is to apply the batch version over an initial set of images, and then to invoke the recursive mode for processing new data. In recursive filtering mode new scene parameters may be incorporated dynamically, and unwanted parameters discarded; hence the term Variable State Dimension Filter. Thus we can handle the introduction of newly visible parts of a scene as the camera/scene moves, and the elimination of obscured or invisible features, while maintaining truly recursive operation. By truly recursive we mean that the amount of computation to process each image depends only on the currently visible features of the scene. At the core of the VSDF is the “recursive partitioning” sparse matrix inversion algorithm invented by the photogrammetrist D.C. Brown and described in the Manual of Photogrammetry [45]. This is a sparse matrix equation solving algorithm, and we shall demonstrate that such sparse methods are necessary to maintain efficient yet accurate reconstruction results. Sparse matrices naturally arise from the nature of individual sensor measurements that involve only specific blocks of parameters. In the VSDF we employ an explicitly block-based version of recursive partitioning, where each block of parameters represents a group of parameters that are to be computed together, such as the 3D coordinates of a scene point, the rotation & translation of a camera, or the internal calibration parameters of a camera. We build the “information” or inverse covariance matrix of the complete system of parameters from the available measurements, and apply Gauss-Newton iterations to solve for an iterative update to minimise the total geometric (projection) error in a least-squares sense. When we turn to recursive operation, we store this inverse covariance, rather than the covariance as is normal with the Kalman filter.