SRBA: Sparser Relative Bundle Adjustment
/home/travis/build/MRPT/srba/examples/cpp/tutorial-srba-how-to-recover-global-map.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 //#define SRBA_DETAILED_TIME_PROFILING   1
00011 
00012 #include <srba.h>
00013 #include <mrpt/random.h>
00014 #include <mrpt/gui.h>  // For rendering results as a 3D scene
00015 
00016 #include <mrpt/graphslam.h> // For global map recovery only
00017 #include <mrpt/opengl/graph_tools.h> // To render the global map
00018 
00019 using namespace srba;
00020 using namespace std;
00021 using mrpt::utils::DEG2RAD;
00022 using mrpt::utils::square;
00023 
00024 
00025 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT
00026 {
00027 //  typedef ecps::local_areas_fixed_size            edge_creation_policy_t;  //!< One of the most important choices: how to construct the relative coordinates graph problem
00028 //  typedef options::sensor_pose_on_robot_none      sensor_pose_on_robot_t;  //!< The sensor pose coincides with the robot pose
00029     typedef options::observation_noise_constant_matrix<observations::RelativePoses_3D>   obs_noise_matrix_t;      // The sensor noise matrix is the same for all observations and equal to some given matrix
00030 //  typedef options::solver_LM_schur_dense_cholesky solver_t;                //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky)
00031 };
00032 
00033 typedef RbaEngine<
00034     kf2kf_poses::SE3,               // Parameterization  of KF-to-KF poses
00035     landmarks::RelativePoses3D,     // Parameterization of landmark positions
00036     observations::RelativePoses_3D, // Type of observations
00037     RBA_OPTIONS
00038     >  my_srba_t;
00039 
00040 // --------------------------------------------------------------------------------
00041 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit )
00042 // --------------------------------------------------------------------------------
00043 const double STD_NOISE_XYZ = 0.05;
00044 const double STD_NOISE_ANGLES = mrpt::utils::DEG2RAD(1.0);
00045 
00046 struct basic_graph_slam_dataset_entry_t
00047 {
00048     unsigned int current_kf;
00049     unsigned int observed_kf;
00050     double x,y,z, yaw,pitch,roll,  qr,qx,qy,qz; // Relative pose of "observed_kf" as seen from "current_kf"
00051 };
00052 basic_graph_slam_dataset_entry_t dataset[] = {
00053  {     1,      0,     -1.78055512,      1.11331694,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00054  {     2,      1,     -1.71545942,      2.05914961,      0.00000000,     -0.37399882,     -0.00000000,      0.00000000,      0.98256650,      0.00000000,     -0.00000000,     -0.18591146},
00055  {     2,      0,     -2.96619160,      3.74601658,      0.00000000,     -0.74799802,     -0.00000000,      0.00000000,      0.93087379,      0.00000000,     -0.00000000,     -0.36534092},
00056  {     3,      2,     -1.15014065,      2.45631509,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00057  {     4,      3,     -0.71839088,      2.17858845,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00058  {     4,      2,     -0.89163376,      4.88530127,      0.00000000,     -0.74799839,     -0.00000000,      0.00000000,      0.93087372,      0.00000000,     -0.00000000,     -0.36534109},
00059  {     5,      4,     -1.33870852,      1.73631597,      0.00000000,     -0.37399882,     -0.00000000,      0.00000000,      0.98256650,      0.00000000,     -0.00000000,     -0.18591146},
00060  {     5,      3,     -1.21151269,      4.02676447,      0.00000000,     -0.74799802,     -0.00000000,      0.00000000,      0.93087379,      0.00000000,     -0.00000000,     -0.36534092},
00061  {     6,      5,     -1.67977719,      2.03565806,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00062  {     6,      4,     -2.29159821,      4.14103420,      0.00000000,     -0.74799802,     -0.00000000,      0.00000000,      0.93087379,      0.00000000,     -0.00000000,     -0.36534092},
00063  {     7,      6,     -1.49006905,      2.30876608,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00064  {     8,      7,     -1.15992524,      2.21845386,      0.00000000,     -0.37399882,     -0.00000000,      0.00000000,      0.98256650,      0.00000000,     -0.00000000,     -0.18591146},
00065  {     9,      8,     -1.28889269,      1.78614744,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00066  {     9,      7,     -1.55814427,      4.27501619,      0.00000000,     -0.74799802,     -0.00000000,      0.00000000,      0.93087379,      0.00000000,     -0.00000000,     -0.36534092},
00067  {    10,      9,     -1.67026750,      1.96210498,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00068  {    10,      8,     -2.21751078,      4.09566815,      0.00000000,     -0.74799839,     -0.00000000,      0.00000000,      0.93087372,      0.00000000,     -0.00000000,     -0.36534109},
00069  {    11,     10,     -1.55210516,      2.27651848,      0.00000000,     -0.37399882,     -0.00000000,      0.00000000,      0.98256650,      0.00000000,     -0.00000000,     -0.18591146},
00070  {    12,     11,     -1.21625554,      2.27164636,      0.00000000,     -0.37399920,     -0.00000000,      0.00000000,      0.98256647,      0.00000000,     -0.00000000,     -0.18591164},
00071  {    13,     12,     -1.45455725,      1.51179033,      0.00000000,     -0.22440012,     -0.00000000,      0.00000000,      0.99371217,      0.00000000,     -0.00000000,     -0.11196480},
00072  {    13,     11,     -2.13482825,      3.99712454,      0.00000000,     -0.59839931,     -0.00000000,      0.00000000,      0.95557270,      0.00000000,     -0.00000000,     -0.29475552},
00073  {    14,     13,     -2.36655195,      0.41536284,      0.00000000,      0.00000000,     -0.00000000,      0.00000000,      1.00000000,      0.00000000,      0.00000000,      0.00000000},
00074  {    14,     12,     -3.82110920,      1.92715317,      0.00000000,     -0.22440012,     -0.00000000,      0.00000000,      0.99371217,      0.00000000,     -0.00000000,     -0.11196480},
00075  {    15,     14,     -2.74448431,     -0.11373775,      0.00000000,      0.00000000,     -0.00000000,      0.00000000,      1.00000000,      0.00000000,      0.00000000,      0.00000000},
00076  {    15,      0,      4.16910212,      0.67638546,      0.00000000,      1.57079633,     -0.00000000,      0.00000000,      0.70710678,      0.00000000,      0.00000000,      0.70710678},
00077  {    16,      0,      1.58658626,      0.30349575,      0.00000000,      1.57079633,     -0.00000000,      0.00000000,      0.70710678,      0.00000000,      0.00000000,      0.70710678},
00078  {    16,     15,     -2.58251586,     -0.37288971,      0.00000000,      0.00000000,     -0.00000000,      0.00000000,      1.00000000,      0.00000000,      0.00000000,      0.00000000},
00079  {    16,      1,      1.97243380,      2.36770815,      0.00000000,      1.94479552,     -0.00000000,      0.00000000,      0.56332003,      0.00000000,      0.00000000,      0.82623879},
00080 };
00081 
00082 int main(int argc, char**argv)
00083 {
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;
00092     //rba.parameters.srba.optimize_new_edges_alone  = false;  // skip optimizing new edges one by one? Relative graph-slam without landmarks should be robust enough, but just to make sure we can leave this to "true" (default)
00093 
00094     // Information matrix for relative pose observations:
00095     {
00096         Eigen::Matrix<double,6,6> ObsL;
00097         ObsL.setZero();
00098         // X,Y,Z:
00099         for (int i=0;i<3;i++) ObsL(i,i) = 1/square(STD_NOISE_XYZ);
00100         // Yaw,pitch,roll:
00101         for (int i=0;i<3;i++) ObsL(3+i,3+i) = 1/square(STD_NOISE_ANGLES);
00102 
00103         // Set:
00104         rba.parameters.obs_noise.lambda = ObsL;
00105     }
00106 
00107     // =========== Topology parameters ===========
00108     rba.parameters.srba.max_tree_depth       = 3;
00109     rba.parameters.srba.max_optimize_depth   = 3;
00110     rba.parameters.ecp.submap_size          = 5;
00111     rba.parameters.ecp.min_obs_to_loop_closure = 1;
00112     // ===========================================
00113 
00114     // --------------------------------------------------------------------------------
00115     // Dump parameters to console (for checking/debugging only)
00116     // --------------------------------------------------------------------------------
00117     cout << "RBA parameters:\n-----------------\n";
00118     rba.parameters.srba.dumpToConsole();
00119 
00120 #if MRPT_HAS_WXWIDGETS
00121     mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
00122 #endif
00123 
00124     // --------------------------------------------------------------------------------
00125     // Process the dataset:
00126     // --------------------------------------------------------------------------------
00127     const size_t nObs = sizeof(dataset)/sizeof(dataset[0]);
00128     size_t cur_kf = 0; // Start at keyframe #0 in the dataset
00129 
00130     for (size_t obsIdx = 0; obsIdx<nObs;  cur_kf++ /* move to next KF */  )
00131     {
00132         // Create list of observations for keyframe: "cur_kf"
00133         my_srba_t::new_kf_observations_t  list_obs;
00134 
00135         // To emulate graph-SLAM, each keyframe MUST have exactly ONE fixed "fake landmark", representing its pose:
00136         // ------------------------------------------------------------------------------------------------------------
00137         {
00138             my_srba_t::new_kf_observation_t obs_field;
00139             obs_field.is_fixed = true;
00140             obs_field.obs.feat_id = cur_kf; // Feature ID == keyframe ID
00141             obs_field.obs.obs_data.x = 0;   // Landmark values are actually ignored.
00142             obs_field.obs.obs_data.y = 0;
00143             obs_field.obs.obs_data.z = 0;
00144             obs_field.obs.obs_data.yaw = 0;
00145             obs_field.obs.obs_data.pitch = 0;
00146             obs_field.obs.obs_data.roll = 0;
00147             list_obs.push_back( obs_field );
00148         }
00149 
00150         // The rest "observations" are real observations of relative poses:
00151         // -----------------------------------------------------------------
00152         while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs )
00153         {
00154             my_srba_t::new_kf_observation_t obs_field;
00155             obs_field.is_fixed = false;   // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated)
00156             obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value.
00157 
00158             obs_field.obs.feat_id      = dataset[obsIdx].observed_kf;
00159             obs_field.obs.obs_data.x   = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ);
00160             obs_field.obs.obs_data.y   = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ);
00161             obs_field.obs.obs_data.z   = dataset[obsIdx].z + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ);
00162             obs_field.obs.obs_data.yaw   = dataset[obsIdx].yaw  + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES);
00163             obs_field.obs.obs_data.pitch = dataset[obsIdx].pitch  + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES);
00164             obs_field.obs.obs_data.roll  = dataset[obsIdx].roll  + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES);
00165 
00166             list_obs.push_back( obs_field );
00167             obsIdx++; // Next dataset entry
00168         }
00169 
00170         //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00171         //  ============================================================================================
00172         my_srba_t::TNewKeyFrameInfo new_kf_info;
00173         rba.define_new_keyframe(
00174             list_obs,      // Input observations for the new KF
00175             new_kf_info,   // Output info
00176             true           // Also run local optimization?
00177             );
00178 
00179         cout << "Created KF #" << new_kf_info.kf_id
00180             << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
00181             << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00182             << "-------------------------------------------------------" << endl;
00183 
00184     // Display:
00185 #if MRPT_HAS_WXWIDGETS
00186         // --------------------------------------------------------------------------------
00187         // Show 3D view of the resulting map:
00188         // --------------------------------------------------------------------------------
00189         my_srba_t::TOpenGLRepresentationOptions  opengl_options;
00190         mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
00191 
00192         rba.build_opengl_representation(
00193             new_kf_info.kf_id ,  // Root KF: the current (latest) KF
00194             opengl_options, // Rendering options
00195             rba_3d  // Output scene
00196             );
00197 
00198         {
00199             mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
00200             scene->clear();
00201             scene->insert(rba_3d);
00202             win.unlockAccess3DScene();
00203         }
00204         win.repaint();
00205 
00206         cout << "Press any key to continue.\n";
00207         win.waitForKey();
00208 #endif
00209 
00210     } // end-for each dataset entry
00211 
00212 
00213     // --------------------------------------------------------------------------------
00214     // Get equivalent graph-slam problem:
00215     // --------------------------------------------------------------------------------
00216     mrpt::graphs::CNetworkOfPoses3D poseGraph;
00217     rba.get_global_graphslam_problem(poseGraph);
00218 
00219     // Run optimization:
00220     mrpt::graphslam::TResultInfoSpaLevMarq out_info;
00221     mrpt::utils::TParametersDouble extra_params;
00222 
00223     mrpt::graphslam::optimize_graph_spa_levmarq(
00224         poseGraph, 
00225         out_info,
00226         NULL, /* in_nodes_to_optimize, NULL=all */
00227         extra_params
00228         );
00229 
00230     // Render resulting graph:
00231 #if MRPT_HAS_WXWIDGETS
00232     mrpt::gui::CDisplayWindow3D win2("Global optimized map",640,480);
00233     {
00234         mrpt::opengl::COpenGLScenePtr &scene = win2.get3DSceneAndLock();
00235 
00236         mrpt::utils::TParametersDouble render_params;   // See docs for mrpt::opengl::graph_tools::graph_visualize()
00237         render_params["show_ID_labels"] = 1;    
00238 
00239         // Get opengl representation of the graph:
00240         mrpt::opengl::CSetOfObjectsPtr gl_global_map = mrpt::opengl::graph_tools::graph_visualize( poseGraph,render_params );
00241         scene->insert(gl_global_map);
00242 
00243         win2.unlockAccess3DScene();
00244         win2.repaint();
00245         mrpt::system::pause();
00246     }
00247     
00248 
00249 #endif
00250 
00251 
00252 
00253     return 0; // All ok
00254 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends