SRBA: Sparser Relative Bundle Adjustment
/home/travis/build/MRPT/srba/examples/cpp/tutorial-srba-range-bearing-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 
00011 #include <srba.h>
00012 #include <mrpt/gui.h>  // For rendering results as a 3D scene
00013 #include <mrpt/random.h>
00014 
00015 using namespace srba;
00016 using namespace mrpt::random;
00017 using namespace std;
00019 
00020 
00022 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT
00023 {
00024 //  typedef ecps::local_areas_fixed_size            edge_creation_policy_t;  //!< One of the most important choices: how to construct the relative coordinates graph problem
00025 //  typedef options::sensor_pose_on_robot_none      sensor_pose_on_robot_t;  //!< The sensor pose coincides with the robot pose
00026 //  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)
00027 //  typedef options::solver_LM_schur_dense_cholesky solver_t;                //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky)
00028 };
00029 
00030 typedef RbaEngine<
00031     kf2kf_poses::SE2,               // Parameterization  of KF-to-KF poses
00032     landmarks::Euclidean2D,     // Parameterization of landmark positions
00033     observations::RangeBearing_2D, // Type of observations
00034     RBA_OPTIONS
00035     >  my_srba_t;
00037 
00038 // --------------------------------------------------------------------------------
00039 // A test dataset. Generated with https://github.com/jlblancoc/recursive-world-toolkit 
00040 //  and the script: tutorials_dataset-range-bearing-2d.cfg
00041 // --------------------------------------------------------------------------------
00042 const double SENSOR_NOISE_STD = 1e-3;
00043 struct basic_range_bearing_dataset_entry_t 
00044 {
00045     unsigned int landmark_id;
00046     double range,yaw,pitch;
00047 };
00048 
00049 // Observations for KF#0. Ground truth pose: (xyz,q)= 0.000000 0.000000 0.000000 0.996195 0.000000 0.000000 0.087156
00050 const mrpt::poses::CPose3DQuat GT_pose0(0,0,0, mrpt::math::CQuaternionDouble(0.996195,0.000000,0.000000,0.087156));
00051 
00052 basic_range_bearing_dataset_entry_t   observations_0[] = {
00053  {    21,   0.74585171,  -1.36075146,   0.00000000},
00054  {    23,   2.02948709,  -1.35571420,   0.00000000},
00055  {    39,   2.68006543,  -0.13563474,   0.00000000},
00056  {    35,   2.86272739,  -0.01644496,   0.00000000},
00057  {    20,   2.91915698,   1.07525990,   0.00000000},
00058  {    28,   3.17296903,  -0.18301471,   0.00000000},
00059  {    18,   3.50125002,  -1.03517885,   0.00000000},
00060  {    37,   3.50752367,  -0.41336794,   0.00000000},
00061  {    45,   3.54994055,  -1.21247025,   0.00000000},
00062   };
00063 // Observations for KF#10:(xyz,q)= 1.230805 0.294472 0.000000 0.999999 0.000000 0.000000 0.001357
00064 const mrpt::poses::CPose3DQuat GT_pose10(1.230805, 0.294472,0.000000, mrpt::math::CQuaternionDouble(0.999999,0.000000,0.000000,0.001357));
00065 basic_range_bearing_dataset_entry_t  observations_10[] = {
00066  {    39,   1.45968442,  -0.13342076,   0.00000000},
00067  {    35,   1.60384967,   0.09483688,   0.00000000},
00068  {    28,   1.96846287,  -0.16671414,   0.00000000},
00069  {    37,   2.45029433,  -0.47938282,   0.00000000},
00070  {    20,   2.49487594,   1.69256088,   0.00000000},
00071  {     6,   2.98119279,  -0.44809197,   0.00000000},
00072  {    18,   3.13128847,  -1.23093971,   0.00000000},
00073  {    13,   3.22347598,   0.32892426,   0.00000000},
00074  {    45,   3.40078719,  -1.40434357,   0.00000000},
00075  {    24,   3.75211822,   0.62688236,   0.00000000},
00076  {     3,   3.83093551,  -0.28399202,   0.00000000},
00077  {    26,   3.84323530,  -0.17313155,   0.00000000},
00078 };
00079 
00080 
00081 int main(int argc, char**argv)
00082 {
00084     my_srba_t rba;     //  Create an empty RBA problem
00085 
00086     // --------------------------------------------------------------------------------
00087     // Set parameters 
00088     // --------------------------------------------------------------------------------
00089     rba.setVerbosityLevel( 1 );   // 0: None; 1:Important only; 2:Verbose
00090 
00091     rba.parameters.srba.use_robust_kernel = false; // true
00092 
00093     rba.parameters.obs_noise.std_noise_observations = 0.05; //SENSOR_NOISE_STD;
00094 
00095     // =========== Topology parameters ===========
00096     rba.parameters.srba.max_tree_depth       =
00097     rba.parameters.srba.max_optimize_depth   = 3;
00098     // ===========================================
00100 
00101     // Set sensors parameters:
00102     // rba.sensor_params has no parameters for the "Cartesian" sensor.
00103     
00104     // Alternatively, parameters can be loaded from an .ini-like config file
00105     // -----------------------------------------------------------------------
00106     // rba.parameters.loadFromConfigFileName("config_file.cfg", "srba");
00107     
00108     // --------------------------------------------------------------------------------
00109     // Dump parameters to console (for checking/debugging only)
00110     // --------------------------------------------------------------------------------
00111     //cout << "RBA parameters:\n-----------------\n";
00112     //rba.parameters.dumpToConsole();
00113 
00115     // --------------------------------------------------------------------------------
00116     // Define observations of KF #0:
00117     // --------------------------------------------------------------------------------
00118     my_srba_t::new_kf_observations_t  list_obs;
00119     my_srba_t::new_kf_observation_t   obs_field;
00120 
00121     obs_field.is_fixed = false;   // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated)
00122     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)
00123 
00124     for (size_t i=0;i<sizeof(observations_0)/sizeof(observations_0[0]);i++)
00125     {
00126         obs_field.obs.feat_id = observations_0[i].landmark_id;
00127         obs_field.obs.obs_data.range = observations_0[i].range + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00128         obs_field.obs.obs_data.yaw = observations_0[i].yaw + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00129         list_obs.push_back( obs_field );
00130     }
00132 
00133 
00135     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00136     //  ============================================================================================
00137     my_srba_t::TNewKeyFrameInfo new_kf_info;
00138     rba.define_new_keyframe(
00139         list_obs,      // Input observations for the new KF
00140         new_kf_info,   // Output info
00141         true           // Also run local optimization?
00142         );
00143 
00144     cout << "Created KF #" << new_kf_info.kf_id 
00145         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
00146         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00147         << "-------------------------------------------------------" << endl;
00149 
00150 
00151     // --------------------------------------------------------------------------------
00152     // Define observations of next KF:
00153     // --------------------------------------------------------------------------------
00154     list_obs.clear();
00155 
00156     for (size_t i=0;i<sizeof(observations_10)/sizeof(observations_10[0]);i++)
00157     {
00158         obs_field.obs.feat_id = observations_10[i].landmark_id;
00159         obs_field.obs.obs_data.range = observations_10[i].range + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00160         obs_field.obs.obs_data.yaw = observations_10[i].yaw + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00161         list_obs.push_back( obs_field );
00162     }
00163     
00164     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00165     //  ============================================================================================
00166     rba.define_new_keyframe(
00167         list_obs,      // Input observations for the new KF
00168         new_kf_info,   // Output info
00169         true           // Also run local optimization?
00170         );
00171 
00172     cout << "Created KF #" << new_kf_info.kf_id 
00173         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size() << endl
00174         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00175         << "-------------------------------------------------------" << endl;
00176 
00177 
00178     // Dump the relative pose of KF#0 wrt KF#1:
00179     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;
00180     cout << "Relative pose of KF#1 wrt KF#0:\n" << (-rba.get_k2k_edges()[0].inv_pose) << endl;
00181 
00182     // Compare to ground truth:
00183     cout << "Ground truth: relative pose of KF#1 wrt KF#0: \n" << mrpt::poses::CPose3D(GT_pose10-GT_pose0) << endl;
00184     
00185 
00187     // --------------------------------------------------------------------------------
00188     // Saving RBA graph as a DOT file:
00189     // --------------------------------------------------------------------------------
00190     const string sFil = "graph.dot";
00191     cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
00192     rba.save_graph_as_dot(sFil, true /* LMs=save */);
00193     cout << "Done.\n";
00195 
00197     // --------------------------------------------------------------------------------
00198     // Show 3D view of the resulting map:
00199     // --------------------------------------------------------------------------------
00200     my_srba_t::TOpenGLRepresentationOptions  opengl_options;
00201     mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
00202 
00203     rba.build_opengl_representation(
00204         0,  // Root KF,
00205         opengl_options, // Rendering options
00206         rba_3d  // Output scene 
00207         );
00208 
00209     // Display:
00210 #if MRPT_HAS_WXWIDGETS
00211     mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
00212     {
00213         mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
00214         scene->insert(rba_3d);
00215         win.unlockAccess3DScene();
00216     }
00217     win.setCameraZoom( 4 ); 
00218     win.repaint();
00219 
00220     cout << "Press any key or close window to exit.\n";
00221     win.waitForKey();
00222 #endif
00223 
00224         
00225     return 0; // All ok
00226 }
00227 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends