SRBA: Sparser Relative Bundle Adjustment
/home/travis/build/MRPT/srba/examples/cpp/tutorial-srba-cartesian3d-se3.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::SE3,             // Parameterization  of KF-to-KF poses
00028     landmarks::Euclidean3D,       // Parameterization of landmark positions
00029     observations::Cartesian_3D,   // 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-4;
00038 struct basic_euclidean_dataset_entry_t 
00039 {
00040     unsigned int landmark_id;
00041     double x,y,z;
00042 };
00043 // Observations for KF#0. Ground truth pose: (xyz,q)=0.000000 0.000000 0.000000 0.996195 0.000000 0.000000 0.087156
00044 const mrpt::poses::CPose3DQuat GT_pose0(0,0,0, mrpt::math::CQuaternionDouble(0.995247,0.003802,-0.043453,0.087073));
00045 basic_euclidean_dataset_entry_t  observations_0[] = {
00046  {    98,   0.44179076,  -0.90376452,   1.21831585},
00047  {   124,   0.36887709,   1.51873558,   0.27846626},
00048  {    61,  -0.13650907,  -1.42614129,   0.84324147},
00049  {    67,   0.60411947,   1.58983767,   0.26060155},
00050  {   143,   0.65546905,  -0.28030347,   1.78930357},
00051  {   146,   1.85201587,   0.02629437,  -0.66017960},
00052  {    21,  -0.01560438,  -0.72945905,  -1.96266939},
00053  {    53,   0.75744877,  -1.97328401,   0.00392723},
00054  {   144,   1.86654080,   0.69846627,  -1.23948292},
00055  {   119,   0.86231273,  -0.88707413,   2.14763103},
00056  {   118,   1.64881194,   1.71511394,  -0.75028619},
00057  {    55,   1.87871121,  -1.47282934,  -0.98887906},
00058  {    64,   1.07097295,  -2.30504332,  -0.70275791},
00059  {    39,   2.62272811,  -0.36239644,  -0.48996278},
00060  {   107,   0.98453078,   2.52728400,   0.54441222},
00061  {    35,   2.81663034,  -0.04707533,  -0.64743974},
00062  {   114,   1.28246133,   1.67306049,   2.05237042},
00063  {    20,   1.34811775,   2.56802258,  -0.51725748},
00064  {    58,  -0.13924954,  -2.72185970,  -1.16368338},
00065  {    63,   0.94986696,   1.81317869,   2.17580006},
00066  };
00067 // Observations for KF#10:(xyz,q)= 1.230806 0.294472 0.000000 0.999999 0.000000 0.000000 0.001357
00068 const mrpt::poses::CPose3DQuat GT_pose10(1.226071, 0.293637, 0.110099, mrpt::math::CQuaternionDouble(0.999103,0.000160,-0.042322,0.001049));
00069 basic_euclidean_dataset_entry_t  observations_10[] = {
00070  {   146,   0.58951282,   0.06112746,  -0.65858034},
00071  {    39,   1.41510972,  -0.19256191,  -0.49100010},
00072  {   144,   0.48965699,   0.73434404,  -1.22816493},
00073  {    67,  -0.90853948,   1.37461702,   0.27968160},
00074  {    35,   1.55245056,   0.15354730,  -0.64321834},
00075  {   124,  -1.12818445,   1.26409439,   0.29563350},
00076  {    98,  -0.64338648,  -1.12385784,   1.20105907},
00077  {   118,   0.10067270,   1.69149623,  -0.72533027},
00078  {   143,  -0.54019814,  -0.48152237,   1.78170215},
00079  {    55,   0.87255815,  -1.40636435,  -1.00855321},
00080  {    28,   1.88660810,  -0.32479903,  -0.78342142},
00081  {    53,  -0.14792658,  -2.10568124,  -0.02727065},
00082  {    61,  -1.12336633,  -1.73181994,   0.81636386},
00083  {    76,   2.16909436,  -0.51355650,   0.02087719},
00084  {    84,   1.37172698,   1.85548870,   0.16019421},
00085  {   141,   1.72910762,  -1.03905231,  -1.19569905},
00086  {   119,  -0.23312637,  -1.04921385,   2.13211166},
00087  {    21,  -1.11968655,  -0.98349037,  -1.97883008},
00088  {    64,   0.21862795,  -2.36852366,  -0.73742358},
00089  {    71,   1.85722805,   0.23574278,   1.64624521},
00090  {   107,  -0.69438136,   2.35901095,   0.57829804},
00091  {    20,  -0.34172474,   2.47692692,  -0.48129000},
00092  {    52,   0.39832690,   2.34432761,   1.03254852},
00093  {   114,  -0.25675973,   1.54618039,   2.07502519},
00094  {    37,   2.25845642,  -1.12777769,   0.83780026},
00095  {    63,  -0.60857470,   1.62555069,   2.19917667},
00096  {    59,   2.57934607,  -1.31716776,  -0.03307248},
00097  {   127,   1.52496362,   1.88008739,  -1.66156675},
00098  };
00099 
00100 int main(int argc, char**argv)
00101 {
00102     my_srba_t rba;     //  Create an empty RBA problem
00103 
00104     // --------------------------------------------------------------------------------
00105     // Set parameters 
00106     // --------------------------------------------------------------------------------
00107     rba.setVerbosityLevel( 1 );   // 0: None; 1:Important only; 2:Verbose
00108 
00109     rba.parameters.srba.use_robust_kernel = true;
00110     rba.parameters.obs_noise.std_noise_observations = 0.1;
00111 
00112     // =========== Topology parameters ===========
00113     rba.parameters.srba.max_tree_depth       = 3;
00114     rba.parameters.srba.max_optimize_depth   = 3;
00115     // ===========================================
00116 
00117     // Set sensors parameters:
00118     // rba.sensor_params has no parameters for the "Cartesian" sensor.
00119     
00120     // Alternatively, parameters can be loaded from an .ini-like config file
00121     // -----------------------------------------------------------------------
00122     // rba.parameters.loadFromConfigFileName("config_file.cfg", "srba");
00123     
00124     // --------------------------------------------------------------------------------
00125     // Dump parameters to console (for checking/debugging only)
00126     // --------------------------------------------------------------------------------
00127     //cout << "RBA parameters:\n-----------------\n";
00128     //rba.parameters.dumpToConsole();
00129 
00130     // --------------------------------------------------------------------------------
00131     // Define observations of KF #0:
00132     // --------------------------------------------------------------------------------
00133     my_srba_t::new_kf_observations_t  list_obs;
00134     my_srba_t::new_kf_observation_t   obs_field;
00135 
00136     obs_field.is_fixed = false;   // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated)
00137     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)
00138 
00139     for (size_t i=0;i<sizeof(observations_0)/sizeof(observations_0[0]);i++)
00140     {
00141         obs_field.obs.feat_id = observations_0[i].landmark_id;
00142         obs_field.obs.obs_data.pt.x = observations_0[i].x + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00143         obs_field.obs.obs_data.pt.y = observations_0[i].y + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00144         obs_field.obs.obs_data.pt.z = observations_0[i].z + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00145         list_obs.push_back( obs_field );
00146     }
00147 
00148 
00149     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00150     //  ============================================================================================
00151     my_srba_t::TNewKeyFrameInfo new_kf_info;
00152     rba.define_new_keyframe(
00153         list_obs,      // Input observations for the new KF
00154         new_kf_info,   // Output info
00155         true           // Also run local optimization?
00156         );
00157 
00158     cout << "Created KF #" << new_kf_info.kf_id 
00159         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
00160         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00161         << "-------------------------------------------------------" << endl;
00162 
00163 
00164     // --------------------------------------------------------------------------------
00165     // Define observations of next KF:
00166     // --------------------------------------------------------------------------------
00167     list_obs.clear();
00168     mrpt::math::CMatrixDouble MAP2( sizeof(observations_10)/sizeof(observations_10[0]), 3);
00169 
00170     for (size_t i=0;i<sizeof(observations_10)/sizeof(observations_10[0]);i++)
00171     {
00172         obs_field.obs.feat_id = observations_10[i].landmark_id;
00173         obs_field.obs.obs_data.pt.x = observations_10[i].x + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00174         obs_field.obs.obs_data.pt.y = observations_10[i].y + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00175         obs_field.obs.obs_data.pt.z = observations_10[i].z + randomGenerator.drawGaussian1D(0,SENSOR_NOISE_STD);
00176         list_obs.push_back( obs_field );
00177     }
00178 
00179     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00180     //  ============================================================================================
00181     rba.define_new_keyframe(
00182         list_obs,      // Input observations for the new KF
00183         new_kf_info,   // Output info
00184         true           // Also run local optimization?
00185         );
00186 
00187     cout << "Created KF #" << new_kf_info.kf_id 
00188         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size() << endl
00189         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00190         << "-------------------------------------------------------" << endl;
00191 
00192 
00193     // Dump the relative pose of KF#0 wrt KF#1:
00194     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;
00195     cout << "Relative pose of KF#1 wrt KF#0:\n" << (-rba.get_k2k_edges()[0].inv_pose) << endl;
00196 
00197     // Compare to ground truth:
00198     cout << "Ground truth: relative pose of KF#1 wrt KF#0: \n" << mrpt::poses::CPose3D(GT_pose10-GT_pose0) << endl;
00199 
00200     // --------------------------------------------------------------------------------
00201     // Saving RBA graph as a DOT file:
00202     // --------------------------------------------------------------------------------
00203     const string sFil = "graph.dot";
00204     cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
00205     rba.save_graph_as_dot(sFil, true /* LMs=save */);
00206     cout << "Done.\n";
00207 
00208     // --------------------------------------------------------------------------------
00209     // Show 3D view of the resulting map:
00210     // --------------------------------------------------------------------------------
00211     my_srba_t::TOpenGLRepresentationOptions  opengl_options;
00212     mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
00213 
00214     rba.build_opengl_representation(
00215         0,  // Root KF,
00216         opengl_options, // Rendering options
00217         rba_3d  // Output scene 
00218         );
00219 
00220     // Display:
00221 #if MRPT_HAS_WXWIDGETS
00222     mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
00223     {
00224         mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
00225         scene->insert(rba_3d);
00226         win.unlockAccess3DScene();
00227     }
00228     win.setCameraZoom( 4 ); 
00229     win.repaint();
00230 
00231     cout << "Press any key or close window to exit.\n";
00232     win.waitForKey();
00233 #endif
00234         
00235     return 0; // All ok
00236 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends