Globally Euclidean Loop Correction

Two opposite ends of the global loop correction spectrum are taken up by Extended Kalman Filter based approaches and bundle adjustment. In EKF approaches, the map and only the latest camera pose are corrected after a loop is detected. In contrast, bundle adjustment tries to create a maximum likelihood estimate of all camera poses and the map. Sub-map based approaches fall in-between these two ends of the spectrum. They generally break the map into a set of many sub-maps. Each of these maps is then corrected separately in their own coordinate frames. Lastly, the sub-maps are held internally fixed and a correction process is performed which minimizes the error of measurements between sub-maps by changing the sub-maps’ relative poses.

Extended Kalman Filter approaches to VSLAM such as Davison’s (2007) only estimate the most recent camera pose. After detecting a loop, the current camera pose and map are updated to reflect the new measurements. If one were to plot the camera’s path based on the EKF estimates, one would see a discontinuity in the camera’s pose just after loop completion, since the previous poses are not updated when the loop is found. This will cause significant problems for procedures that rely on an accurate camera path, including dense stereo matching and scene reconstruction.

Bundle adjustment (Triggs et al., 2000) represents the opposite end of loop correction spectrum. Using a non-linear minimization, bundle adjustment seeks to find the globally optimal camera path and 3D scene structure given the images in a video sequence and feature correspondences between the frames. Many different parameterizations of bundle adjustment exist, but the basic form of the bundle adjustment error term is

i

j

d(x_ij,Π(R_i, C_i, X_j))2 (2.2)

where iis the camera index, j is the 3D feature index,R_i is the rotation of camera i, C_i is the center of camerai, X_j is the three-dimensional position of feature j, x_ij is the

measured value of featurej in camerai, Πmodels the projection of the 3D point into the camera, andd()is a measure of the distance between the measured and expected projec- tion of the feature and is generally given in image pixels. Typically, bundle adjustment procedures minimize this sum of squared errors using a Levenberg-Marquardt algorithm (Levenberg, 1944; Marquardt, 1963). However, others have shown promising results using preconditioned conjugate gradients (Byrod and Astrom, 2009; Dellaert et al., 2010).

Bundle adjustment is a highly sparse minimization problem. Each expected measure- ment is only affected by a single camera and 3D point. This gives the Jaccobian of the pro- jection functionΠits sparse structure. Lourakis and Argyros (2009) take advantage of this structure in their open source implementation sparse bundle adjustment, or . The bundle adjustment problem state can be partitioned into cameras and points. The Schur comple- ment can be used to factor out the points, converting their non-zeros in the full Jaccobian matrix into constraints between cameras that both see the same feature. The features are generally factored out rather than the cameras because there are many more features than cameras in a typical bundle adjustment problem. However, in a situation where a camera moves in the same environment for a long period of time, it may be more efficient to factor out the cameras if there are more camera poses than points. At each iteration of the min- imization, the Schur complement is used to factor out the features, the camera portion of the state is updated based on the measurement residuals, and finally the updated 3D feature positions are calculated.

Bundle adjustment re-linearizes the measurement functions in each update step. In contrast, the EKF linearizes the measurements only once about the state estimate of the feature and camera at the time of the measurement. The influence of a given measurement is then folded into the EKF’s pose and map estimates as well as their uncertainties. The problem with this approach is that if the linearization point was far away from the true position the EKF will not be able to recover from this error. In contrast, starting from a state somewhat far from the global minimum of the bundle adjustment cost function

(Equation 2.2), re-linearization can allow bundle adjustment to converge to the correct solution where an EKF will not. Of course, the error function has local minima in which the optimization may get stuck.

In addition to problems with local minima,due to its least squares formulation, bundle adjustment cannot deal directly with outlier measurements. However, a lack of robustness is a problem for any least-squares framework, including the EKF. Robust bundle adjustment methods try to deal with outliers by down-weighing their influence in the minimization. A simple method to do this is to assign a higher uncertainty to measurements that exhibit large residual errors (McGlone et al., 2004). Other approaches involve down-weighing the error term for measurements with large residuals according to some function (McGlone et al., 2004).

Bundle adjustment can be extended to other types of sensors besides cameras. Thrun et al. describe the GraphSLAM algorithm in their book Probabilistic Robotics (Thrun et al., 2005). The GraphSLAM algorithm generalizes bundle adjustment to include robotic con- trol inputs, odometry information, and any other type of sensor information that can be used to map a pose of the robot or camera to another pose or a pose to a feature in the world.