SRBA: Sparser Relative Bundle Adjustment
/home/travis/build/MRPT/srba/examples/cpp/tutorial-srba-cartesian2d-se2.cpp
00001 /* +---------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)               |
00003    |                          http://www.mrpt.org/                             |
00004    |                                                                           |
00005    | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00007    | Released under BSD License. See details in http://www.mrpt.org/License    |
00008    +---------------------------------------------------------------------------+ */
00009 
00010 #include <srba.h>
00011 #include <mrpt/gui.h>  // For rendering results as a 3D scene
00012 #include <mrpt/random.h>
00013 
00014 using namespace srba;
00015 using namespace mrpt::random;
00016 using namespace std;
00017 
00018 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT
00019 {
00020 //  typedef ecps::local_areas_fixed_size            edge_creation_policy_t;  //!< One of the most important choices: how to construct the relative coordinates graph problem
00021 //  typedef options::sensor_pose_on_robot_none      sensor_pose_on_robot_t;  //!< The sensor pose coincides with the robot pose
00022 //  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)
00023 //  typedef options::solver_LM_schur_dense_cholesky solver_t;                //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky)
00024 };
00025 
00026 typedef RbaEngine<
00027     kf2kf_poses::SE2,             // Parameterization  of KF-to-KF poses
00028     landmarks::Euclidean2D,       // Parameterization of landmark positions
00029     observations::Cartesian_2D,   // Type of observations
00030     RBA_OPTIONS
00031     >  my_srba_t;
00032 
00033 // --------------------------------------------------------------------------------
00034 // A test dataset. Generated with https://github.com/jlblancoc/recursive-world-toolkit
00035 //  and the script: tutorials_dataset-cartesian.cfg
00036 // --------------------------------------------------------------------------------
00037 const double SENSOR_NOISE_STD = 1e-2;
00038 
00039 struct basic_euclidean_dataset_entry_t
00040 {
00041     unsigned int landmark_id;
00042     double x,y,z;
00043 };
00044 // Observations for KF#0
00045 const mrpt::poses::CPose3DQuat GT_pose0(0,0,0, mrpt::math::CQuaternionDouble( 0.996195,0.,0.,0.087156));
00046 
00047 basic_euclidean_dataset_entry_t  observations_0[] = {
00048  {    21,   0.15551290,  -0.72945905,   0.00000000},
00049  {    23,   0.43314868,  -1.98272542,   0.00000000},
00050  {    39,   2.65545091,  -0.36239644,   0.00000000},
00051  {    35,   2.86234031,  -0.04707532,   0.00000000},
00052  {    20,   1.38806971,   2.56802258,   0.00000000},
00053  {    28,   3.11997887,  -0.57746373,   0.00000000},
00054  {    18,   1.78694047,  -3.01091272,   0.00000000},
00055  {    37,   3.21209594,  -1.40895775,   0.00000000},
00056  {    45,   1.24498942,  -3.32446676,   0.00000000},
00057  {     6,   3.68972255,  -1.65569177,   0.00000000},
00058  {    13,   4.44701777,   0.58061391,   0.00000000},
00059  {    43,   1.00330919,  -4.40925967,   0.00000000},
00060  {    30,   1.78741597,  -4.28620926,   0.00000000},
00061  {    24,   4.63351881,   1.72539759,   0.00000000},
00062  {     3,   4.70305332,  -1.61007267,   0.00000000},
00063  };
00064 // Observations for KF#10:
00065 const mrpt::poses::CPose3DQuat GT_pose10(1.230806,0.294472,0, mrpt::math::CQuaternionDouble(  0.999999,0.,0.,0.001357));
00066 
00067 basic_euclidean_dataset_entry_t  observations_10[] = {
00068  {    21,  -0.95365746,  -0.98326083,   0.00000000},
00069  {    39,   1.44671094,  -0.19417495,   0.00000000},
00070  {    35,   1.59664177,   0.15187617,   0.00000000},
00071  {    28,   1.94117010,  -0.32665253,   0.00000000},
00072  {    23,  -0.46583194,  -2.17060428,   0.00000000},
00073  {    37,   2.17409589,  -1.13015327,   0.00000000},
00074  {    20,  -0.30303829,   2.47640333,   0.00000000},
00075  {     6,   2.68687505,  -1.29159175,   0.00000000},
00076  {    18,   1.04382049,  -2.95218658,   0.00000000},
00077  {    13,   3.05066582,   1.04126389,   0.00000000},
00078  {    45,   0.56345958,  -3.35378398,   0.00000000},
00079  {    24,   3.03869071,   2.20107806,   0.00000000},
00080  {     3,   3.67748505,  -1.07338963,   0.00000000},
00081  {    26,   3.78577874,  -0.66206601,   0.00000000},
00082  {    32,   3.96151904,  -0.93661778,   0.00000000},
00083  {     8,   3.14718105,  -2.85542922,   0.00000000},
00084  {     1,   3.04568235,  -3.03423259,   0.00000000},
00085  {    30,   1.26233346,  -4.20862344,   0.00000000},
00086  {    12,   4.08965501,   1.81773981,   0.00000000},
00087  {    43,   0.51081104,  -4.46392502,   0.00000000},
00088  {     5,   4.56480438,  -0.49754058,   0.00000000},
00089  {     2,   4.28747355,   1.88540463,   0.00000000},
00090  {    11,   3.16214953,  -3.54854757,   0.00000000},
00091  };
00092 
00093 int main(int argc, char**argv)
00094 {
00095     my_srba_t rba;     //  Create an empty RBA problem
00096 
00097     // --------------------------------------------------------------------------------
00098     // Set parameters
00099     // --------------------------------------------------------------------------------
00100     rba.setVerbosityLevel( 1 );   // 0: None; 1:Important only; 2:Verbose
00101 
00102     rba.parameters.srba.use_robust_kernel= true;
00103     rba.parameters.obs_noise.std_noise_observations = 0.1;
00104 
00105     // =========== Topology parameters ===========
00106     rba.parameters.srba.max_tree_depth       = 3;
00107     rba.parameters.srba.max_optimize_depth   = 3;
00108     // ===========================================
00109 
00110     // Set sensors parameters:
00111     // rba.sensor_params has no parameters for the "Cartesian" sensor.
00112 
00113     // Alternatively, parameters can be loaded from an .ini-like config file
00114     // -----------------------------------------------------------------------
00115     // rba.parameters.loadFromConfigFileName("config_file.cfg", "srba");
00116 
00117     // --------------------------------------------------------------------------------
00118     // Dump parameters to console (for checking/debugging only)
00119     // --------------------------------------------------------------------------------
00120     //cout << "RBA parameters:\n-----------------\n";
00121     //rba.parameters.dumpToConsole();
00122 
00123     // --------------------------------------------------------------------------------
00124     // Define observations of KF #0:
00125     // --------------------------------------------------------------------------------
00126     my_srba_t::new_kf_observations_t  list_obs;
00127     my_srba_t::new_kf_observation_t   obs_field;
00128 
00129     obs_field.is_fixed = false;   // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated)
00130     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)
00131 
00132     for (size_t i=0;i<sizeof(observations_0)/sizeof(observations_0[0]);i++)
00133     {
00134         obs_field.obs.feat_id = observations_0[i].landmark_id;
00135         obs_field.obs.obs_data.pt.x = observations_0[i].x + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00136         obs_field.obs.obs_data.pt.y = observations_0[i].y + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00137         list_obs.push_back( obs_field );
00138     }
00139 
00140 
00141     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00142     //  ============================================================================================
00143     my_srba_t::TNewKeyFrameInfo new_kf_info;
00144     rba.define_new_keyframe(
00145         list_obs,      // Input observations for the new KF
00146         new_kf_info,   // Output info
00147         true           // Also run local optimization?
00148         );
00149 
00150     cout << "Created KF #" << new_kf_info.kf_id
00151         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
00152         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00153         << "-------------------------------------------------------" << endl;
00154 
00155 
00156     // --------------------------------------------------------------------------------
00157     // Define observations of next KF:
00158     // --------------------------------------------------------------------------------
00159     list_obs.clear();
00160 
00161     for (size_t i=0;i<sizeof(observations_10)/sizeof(observations_10[0]);i++)
00162     {
00163         obs_field.obs.feat_id = observations_10[i].landmark_id;
00164         obs_field.obs.obs_data.pt.x = observations_10[i].x + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00165         obs_field.obs.obs_data.pt.y = observations_10[i].y + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00166         list_obs.push_back( obs_field );
00167     }
00168 
00169     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00170     //  ============================================================================================
00171     rba.define_new_keyframe(
00172         list_obs,      // Input observations for the new KF
00173         new_kf_info,   // Output info
00174         true           // Also run local optimization?
00175         );
00176 
00177     cout << "Created KF #" << new_kf_info.kf_id
00178         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size() << endl
00179         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00180         << "-------------------------------------------------------" << endl;
00181 
00182 
00183     // Dump the relative pose of KF#0 wrt KF#1:
00184     cout << "inv_pose of KF-to-KF edge #0 (relative pose of KF#0 wrt KF#1):\n" << rba.get_k2k_edges()[0].inv_pose << endl;
00185     cout << "Relative pose of KF#1 wrt KF#0:\n" << (-rba.get_k2k_edges()[0].inv_pose) << endl;
00186 
00187     // Compare to ground truth:
00188     cout << "Ground truth: relative pose of KF#1 wrt KF#0: \n" << mrpt::poses::CPose3D(GT_pose10-GT_pose0) << endl;
00189 
00190     // --------------------------------------------------------------------------------
00191     // Saving RBA graph as a DOT file:
00192     // --------------------------------------------------------------------------------
00193     const string sFil = "graph.dot";
00194     cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
00195     rba.save_graph_as_dot(sFil, true /* LMs=save */);
00196     cout << "Done.\n";
00197 
00198     // --------------------------------------------------------------------------------
00199     // Show 3D view of the resulting map:
00200     // --------------------------------------------------------------------------------
00201     my_srba_t::TOpenGLRepresentationOptions  opengl_options;
00202     mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
00203 
00204     rba.build_opengl_representation(
00205         0,  // Root KF,
00206         opengl_options, // Rendering options
00207         rba_3d  // Output scene
00208         );
00209 
00210     // Display:
00211 #if MRPT_HAS_WXWIDGETS
00212     mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
00213     {
00214         mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
00215         scene->insert(rba_3d);
00216         win.unlockAccess3DScene();
00217     }
00218     win.setCameraZoom( 4 );
00219     win.repaint();
00220 
00221     cout << "Press any key or close window to exit.\n";
00222     win.waitForKey();
00223 #endif
00224 
00225     return 0; // All ok
00226 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends