It took me a while to really understand the structure of the okvis SLAM system. In order to prevent from studying its structure all over again, it is better for me to put down what I understand so far.

The main goal of this post is to bridge the gap between the theoretical understanding and the sophisticated implementation.

To begin with

Let's first look at okvis_app_synchronous.cpp. It first reads the configuration file, and then initialize an okvis::ThreadedKFVio instance, which can be considered as a SLAM system.

okvis::VioParameters parameters; // 220
okvis::ThreadedKFVio okvis_estimator(parameters); // 223

Therefore, everything happens in okvis::ThreadedKFVio. In ThreadedKFVio.hpp, there are two main private members.

okvis::Estimator estimator_;    ///< The backend estimator.
okvis::Frontend frontend_;    ///< The frontend.

The main two blocks of a SLAM system are implemented in these two members, respectively.

A NLS SLAM basically solves the following optimization problem: [(\hat{s}{1:n}, \hat{\lambda}) = {arg\,min}{({s}{1:n}, {\lambda})}\, \sum{t=0}^{n-1} | s_{t+1} - f(s_t, ut) |^2{Q} + \sum_{t=1}^n | o_t - h(st, \lambda) |^2{R}. ] Since the optimization is carried out by Ceres, we now need to know when the time propagation constraints and the observation constraints are added in the system.

Observation constraints

In Frontend::dataAssociationAndInitialization(), different matches take place regarding the pair of frames and the camera geometry. In particular, there are 3 matches: matchToKeyframes(), matchToLastFrame(), and matchStereo(). All these 3 match functions use VioKeyframeWindowMatchingAlgorithm.

The constructor of VioKeyframeWindowMatchingAlgorithm takes estimator as input, and the observation constraints are added here.

In VioKeyframeWindowMatchingAlgorithm.cpp,

estimator_->addObservation<camera_geometry_t>(lmId, mfIdA_, camIdA_, indexA);  // 457

Now we turn to see the implementation of Estimator::addObservation() in implementation/Estimator.hpp. We can see that cost function is constructed, and then is added to the optimization problem. Here, the cost function is provided by ReprojectionError.

The calculation of residuals and Jacobian matrices of ReprojectionError is in implmentation/Reprojection.hpp. Especially in ReproejctionError::Evaluate(), the projection function from the camera is called.

The projection and the distortion is realized in PinholeCamera. In their implementation, a 3d point is first projected to 2d, then is distorted, and finally scaled and offset in the image.

In summary, we trace the implementation by following: Frontend -> VioKeyframeWindowMatchingAlgorithm -> Estimator -> ReprojectionError -> PinholeCamera.

Next Post Previous Post