SRBA

1. Description

Sparser Relative Bundle Adjustment (SRBA): a C++ framework for relative SLAM:

See https://github.com/MRPT/srba and "<i>The SRBA guide</i>" (PDF) for references and further theoretical and coding information.

2. API reference

The main classes are:

• srba::RbaEngine

3. C++ Examples

All demos can be found under the directory `examples/cpp`. Compile them by enabling the CMake flag `BUILD_EXAMPLES`.

• Range-Bearing 2D SLAM: cpp/tutorial-srba-range-bearing-se2.cpp
• Range-Bearing 3D SLAM: cpp/tutorial-srba-range-bearing-se3.cpp
• Cartesian sensor 2D SLAM: cpp/tutorial-srba-cartesian2d-se2.cpp
• Cartesian sensor 3D SLAM: cpp/tutorial-srba-cartesian2d-se2.cpp
• Relative graph-SLAM 2D: cpp/tutorial-srba-relative-graph-slam-se2.cpp
• Relative graph-SLAM 3D: cpp/tutorial-srba-relative-graph-slam-se3.cpp
• Stereo visual SLAM 2D: cpp/tutorial-srba-stereo-se2.cpp
• Stereo visual SLAM 3D: cpp/tutorial-srba-stereo-se3.cpp
• How to recover the global map: cpp/tutorial-srba-how-to-recover-global-map.cpp

4. Example SLAM code explained

• See full source code in: cpp/tutorial-srba-range-bearing-se2.cpp

First, include SRBA headers and declare the intention to use symbols under namespace `srba`:

#include <srba.h>
#include <mrpt/gui.h>  // For rendering results as a 3D scene
#include <mrpt/random.h>

using namespace srba;
using namespace mrpt::random;
using namespace std;

Now, configure your SLAM problem by defining all the required template arguments:

struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT
{
//  typedef ecps::local_areas_fixed_size            edge_creation_policy_t;  //!< One of the most important choices: how to construct the relative coordinates graph problem
//  typedef options::sensor_pose_on_robot_none      sensor_pose_on_robot_t;  //!< The sensor pose coincides with the robot pose
//  typedef options::observation_noise_identity     obs_noise_matrix_t;      //!< The sensor noise matrix is the same for all observations and equal to \sigma * I(identity)
//  typedef options::solver_LM_schur_dense_cholesky solver_t;                //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky)
};

typedef RbaEngine<
    kf2kf_poses::SE2,               // Parameterization  of KF-to-KF poses
    landmarks::Euclidean2D,     // Parameterization of landmark positions
    observations::RangeBearing_2D, // Type of observations
    RBA_OPTIONS
    >  my_srba_t;

Next, declare the SRBA object instance and set up the main algorithm parameters:

    my_srba_t rba;     //  Create an empty RBA problem

    // --------------------------------------------------------------------------------
    // Set parameters 
    // --------------------------------------------------------------------------------
    rba.setVerbosityLevel( 1 );   // 0: None; 1:Important only; 2:Verbose

    rba.parameters.srba.use_robust_kernel = false; // true

    rba.parameters.obs_noise.std_noise_observations = 0.05; //SENSOR_NOISE_STD;

    // =========== Topology parameters ===========
    rba.parameters.srba.max_tree_depth       =
    rba.parameters.srba.max_optimize_depth   = 3;
    // ===========================================

Once you want to create a new Keyframe, first create the corresponding sensory observations:

    // --------------------------------------------------------------------------------
    // Define observations of KF #0:
    // --------------------------------------------------------------------------------
    my_srba_t::new_kf_observations_t  list_obs;
    my_srba_t::new_kf_observation_t   obs_field;

    obs_field.is_fixed = false;   // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated)
    obs_field.is_unknown_with_init_val = false; // We don't have any guess on the initial LM position (will invoke the inverse sensor model)

    for (size_t i=0;i<sizeof(observations_0)/sizeof(observations_0[0]);i++)
    {
        obs_field.obs.feat_id = observations_0[i].landmark_id;
        obs_field.obs.obs_data.range = observations_0[i].range + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
        obs_field.obs.obs_data.yaw = observations_0[i].yaw + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
        list_obs.push_back( obs_field );
    }

then call srba::RbaEngine::define_new_keyframe() to create the new KeyFrame and optimize the local area:

    //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
    //  ============================================================================================
    my_srba_t::TNewKeyFrameInfo new_kf_info;
    rba.define_new_keyframe(
        list_obs,      // Input observations for the new KF
        new_kf_info,   // Output info
        true           // Also run local optimization?
        );

    cout << "Created KF #" << new_kf_info.kf_id 
        << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
        << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
        << "-------------------------------------------------------" << endl;

Repeat the last two steps (create observations, define keyframes) for each new Keyframe.

At any moment you can create a 3D view of the SLAM state:

    // --------------------------------------------------------------------------------
    // Show 3D view of the resulting map:
    // --------------------------------------------------------------------------------
    my_srba_t::TOpenGLRepresentationOptions  opengl_options;
    mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();

    rba.build_opengl_representation(
        0,  // Root KF,
        opengl_options, // Rendering options
        rba_3d  // Output scene 
        );

    // Display:
#if MRPT_HAS_WXWIDGETS
    mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
    {
        mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
        scene->insert(rba_3d);
        win.unlockAccess3DScene();
    }
    win.setCameraZoom( 4 ); 
    win.repaint();

    cout << "Press any key or close window to exit.\n";
    win.waitForKey();
#endif

or save the graph in Graphviz DOT format for debugging:

    // --------------------------------------------------------------------------------
    // Saving RBA graph as a DOT file:
    // --------------------------------------------------------------------------------
    const string sFil = "graph.dot";
    cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
    rba.save_graph_as_dot(sFil, true /* LMs=save */);
    cout << "Done.\n";