SRBA: Sparser Relative Bundle Adjustment
/home/travis/build/MRPT/srba/examples/cpp/tutorial-srba-stereo-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 
00013 using namespace srba;
00014 using namespace std;
00015 using mrpt::utils::DEG2RAD;
00016 
00017 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT
00018 {
00019 //  typedef ecps::local_areas_fixed_size            edge_creation_policy_t;  //!< One of the most important choices: how to construct the relative coordinates graph problem
00020     typedef options::sensor_pose_on_robot_se3        sensor_pose_on_robot_t;
00021     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)
00022 //  typedef options::solver_LM_schur_dense_cholesky solver_t;                //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky)
00023 };
00024 
00025 typedef RbaEngine<
00026     kf2kf_poses::SE2,              // Parameterization  of KF-to-KF poses
00027     landmarks::Euclidean3D,        // Parameterization of landmark positions
00028     observations::StereoCamera, // Type of observations
00029     RBA_OPTIONS
00030     >  my_srba_t;
00031 
00032 // --------------------------------------------------------------------------------
00033 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit )
00034 // --------------------------------------------------------------------------------
00035 struct basic_stereo_dataset_entry_t
00036 {
00037     unsigned int landmark_id;
00038     double l_px_x, l_px_y, r_px_x, r_px_y;
00039 };
00040 // Observations for KF#0
00041 basic_stereo_dataset_entry_t  dataset0[] = {
00042  {    39,  539.29449463,  384.00000000,  524.23114014,  384.00000000},
00043  {    35,  515.28930664,  384.00000000,  501.31469727,  384.00000000},
00044  {    20,  141.98646545,  384.00000000,  113.16947174,  384.00000000},
00045  {    28,  549.01715088,  384.00000000,  536.19653320,  384.00000000},
00046  {    18,  848.99078369,  384.00000000,  826.60614014,  384.00000000},
00047  {    37,  599.72821045,  384.00000000,  587.27532959,  384.00000000},
00048  {     6,  601.74615479,  384.00000000,  590.90521240,  384.00000000},
00049  {    13,  485.88748169,  384.00000000,  476.89270020,  384.00000000},
00050  {    30,  991.59832764,  384.00000000,  969.21966553,  384.00000000},
00051  {    24,  437.52539063,  384.00000000,  428.89263916,  384.00000000},
00052  {     3,  580.46923828,  384.00000000,  571.96411133,  384.00000000},
00053  {    26,  562.13531494,  384.00000000,  553.93872070,  384.00000000},
00054  {     8,  681.00463867,  384.00000000,  670.68438721,  384.00000000},
00055  {     1,  695.37768555,  384.00000000,  684.69763184,  384.00000000},
00056  {    32,  572.87854004,  384.00000000,  564.88854980,  384.00000000},
00057  {    11,  721.99755859,  384.00000000,  711.39343262,  384.00000000},
00058  {    12,  470.31097412,  384.00000000,  463.17254639,  384.00000000},
00059  {     5,  554.08819580,  384.00000000,  547.04064941,  384.00000000},
00060  {     2,  470.66183472,  384.00000000,  463.77709961,  384.00000000},
00061  {    36,  941.68115234,  384.00000000,  925.83496094,  384.00000000},
00062  {    22,  712.12817383,  384.00000000,  702.74102783,  384.00000000},
00063  {    46,  660.98260498,  384.00000000,  653.19744873,  384.00000000},
00064  {    47,  412.31518555,  384.00000000,  405.41717529,  384.00000000},
00065  {     7,  476.52420044,  384.00000000,  470.44110107,  384.00000000},
00066  {    17,  592.48101807,  384.00000000,  586.03741455,  384.00000000},
00067  {    42,  538.14855957,  384.00000000,  532.17541504,  384.00000000},
00068  {    27,  705.59130859,  384.00000000,  697.47851563,  384.00000000},
00069  {    31,  374.95474243,  384.00000000,  367.96514893,  384.00000000},
00070 };
00071 
00072 // Observations for KF#1. GT pose: 10 -> 0.502744 -0.099901 0.000000 0.526224 -0.526224 0.472322 -0.472322
00073 basic_stereo_dataset_entry_t  dataset1[] = {
00074  {    39,  538.84362793,  384.00000000,  511.19470215,  384.00000000},
00075  {    35,  492.97552490,  384.00000000,  467.92294312,  384.00000000},
00076  {    28,  545.65521240,  384.00000000,  525.04907227,  384.00000000},
00077  {    37,  615.96533203,  384.00000000,  597.56689453,  384.00000000},
00078  {     6,  608.14080811,  384.00000000,  593.25360107,  384.00000000},
00079  {    13,  443.73529053,  384.00000000,  430.62341309,  384.00000000},
00080  {    24,  367.12982178,  384.00000000,  353.96624756,  384.00000000},
00081  {     3,  570.37628174,  384.00000000,  559.49926758,  384.00000000},
00082  {    26,  546.97644043,  384.00000000,  536.41058350,  384.00000000},
00083  {    32,  559.28576660,  384.00000000,  549.18865967,  384.00000000},
00084  {     8,  693.45947266,  384.00000000,  680.74969482,  384.00000000},
00085  {     1,  711.24810791,  384.00000000,  698.11474609,  384.00000000},
00086  {    12,  423.10546875,  384.00000000,  413.32467651,  384.00000000},
00087  {     5,  533.79895020,  384.00000000,  525.03625488,  384.00000000},
00088  {     2,  424.05053711,  384.00000000,  414.72103882,  384.00000000},
00089  {    11,  736.43890381,  384.00000000,  723.78930664,  384.00000000},
00090  {    22,  715.67138672,  384.00000000,  704.84838867,  384.00000000},
00091  {    47,  334.04904175,  384.00000000,  324.01797485,  384.00000000},
00092  {     7,  433.45681763,  384.00000000,  425.53250122,  384.00000000},
00093  {    46,  653.96527100,  384.00000000,  645.04693604,  384.00000000},
00094  {    42,  512.31976318,  384.00000000,  505.06820679,  384.00000000},
00095  {    17,  575.66925049,  384.00000000,  568.13891602,  384.00000000},
00096  {    36,  988.72906494,  384.00000000,  970.40332031,  384.00000000},
00097  {     9,  558.54290771,  384.00000000,  551.54156494,  384.00000000},
00098  {    31,  268.40109253,  384.00000000,  257.69583130,  384.00000000},
00099  {     0,  448.58941650,  384.00000000,  441.61938477,  384.00000000},
00100  {    40,  608.29187012,  384.00000000,  600.95947266,  384.00000000},
00101  {    27,  698.85803223,  384.00000000,  689.85357666,  384.00000000},
00102  {    16,  550.01416016,  384.00000000,  543.33697510,  384.00000000},
00103  {    15,  486.11828613,  384.00000000,  479.63989258,  384.00000000},
00104  {    10,  479.26995850,  384.00000000,  472.76791382,  384.00000000},
00105  {    48,  358.30957031,  384.00000000,  350.39227295,  384.00000000},
00106  {    38,  390.95483398,  384.00000000,  383.80099487,  384.00000000},
00107  {    29,  392.46899414,  384.00000000,  385.34008789,  384.00000000},
00108 };
00109 
00110 int main(int argc, char**argv)
00111 {
00112     my_srba_t rba;     //  Create an empty RBA problem
00113 
00114     // --------------------------------------------------------------------------------
00115     // Set parameters
00116     // --------------------------------------------------------------------------------
00117     rba.setVerbosityLevel( 1 );   // 0: None; 1:Important only; 2:Verbose
00118 
00119     rba.parameters.srba.use_robust_kernel = true;
00120     rba.parameters.obs_noise.std_noise_observations = 0.5; // pixels
00121 
00122     // =========== Topology parameters ===========
00123     rba.parameters.srba.max_tree_depth       = 3;
00124     rba.parameters.srba.max_optimize_depth   = 3;
00125     // ===========================================
00126 
00127     // Set camera calib:
00128     mrpt::utils::TCamera & lc = rba.parameters.sensor.camera_calib.leftCamera;
00129     lc.ncols = 1024;
00130     lc.nrows = 768;
00131     lc.cx(512);
00132     lc.cy(384);
00133     lc.fx(200);
00134     lc.fy(150);
00135     lc.dist.setZero();
00136     rba.parameters.sensor.camera_calib.rightCamera = lc;
00137     rba.parameters.sensor.camera_calib.rightCameraPose.fromString("[0.2 0 0  1 0 0 0]");  // [X Y Z qr qx qy qz]
00138 
00139     // Sensor pose on the robot parameters:
00140     rba.parameters.sensor_pose.relative_pose = mrpt::poses::CPose3D(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90) ); // Set camera pointing forwards (camera's +Z is robot +X)
00141 
00142     // Alternatively, parameters can be loaded from an .ini-like config file
00143     // -----------------------------------------------------------------------
00144     // rba.parameters.loadFromConfigFileName("config_file.cfg", "srba");
00145     //rba.sensor_params.camera_calib.loadFromConfigFile("CAMERA","config_file.cfg");
00146 
00147     // --------------------------------------------------------------------------------
00148     // Dump parameters to console (for checking/debugging only)
00149     // --------------------------------------------------------------------------------
00150     //cout << "RBA parameters:\n-----------------\n";
00151     //rba.parameters.dumpToConsole();
00152 
00153 
00154     // --------------------------------------------------------------------------------
00155     // Define observations of KF #0:
00156     // --------------------------------------------------------------------------------
00157     my_srba_t::new_kf_observations_t  list_obs;
00158     my_srba_t::new_kf_observation_t   obs_field;
00159 
00160     obs_field.is_fixed = false;   // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated)
00161 #if 1
00162     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)
00163 #else
00164     obs_field.is_unknown_with_init_val = true;      // Set default relative position of unknown new landmarks...
00165     obs_field.setRelPos( mrpt::math::TPoint3D(0.,0.,1.) ); // ...to (x,y,z)=(0,0,1), in front of the camera.
00166 #endif
00167 
00168     for (size_t i=0;i<sizeof(dataset0)/sizeof(dataset0[0]);i++)
00169     {
00170         obs_field.obs.feat_id = dataset0[i].landmark_id;
00171         obs_field.obs.obs_data.left_px.x = dataset0[i].l_px_x;
00172         obs_field.obs.obs_data.left_px.y = dataset0[i].l_px_y;
00173         obs_field.obs.obs_data.right_px.x = dataset0[i].r_px_x;
00174         obs_field.obs.obs_data.right_px.y = dataset0[i].r_px_y;
00175         list_obs.push_back( obs_field );
00176     }
00177 
00178 
00179     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00180     //  ============================================================================================
00181     my_srba_t::TNewKeyFrameInfo new_kf_info;
00182     rba.define_new_keyframe(
00183         list_obs,      // Input observations for the new KF
00184         new_kf_info,   // Output info
00185         true           // Also run local optimization?
00186         );
00187 
00188     cout << "Created KF #" << new_kf_info.kf_id
00189         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size()  << endl
00190         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00191         << "-------------------------------------------------------" << endl;
00192 
00193 
00194     // --------------------------------------------------------------------------------
00195     // Define observations of KF #1:
00196     // --------------------------------------------------------------------------------
00197     list_obs.clear();
00198 
00199     for (size_t i=0;i<sizeof(dataset1)/sizeof(dataset1[0]);i++)
00200     {
00201         obs_field.obs.feat_id = dataset1[i].landmark_id;
00202         obs_field.obs.obs_data.left_px.x = dataset1[i].l_px_x;
00203         obs_field.obs.obs_data.left_px.y = dataset1[i].l_px_y;
00204         obs_field.obs.obs_data.right_px.x = dataset1[i].r_px_x;
00205         obs_field.obs.obs_data.right_px.y = dataset1[i].r_px_y;
00206         list_obs.push_back( obs_field );
00207     }
00208 
00209     //  Here happens the main stuff: create Key-frames, build structures, run optimization, etc.
00210     //  ============================================================================================
00211     rba.define_new_keyframe(
00212         list_obs,      // Input observations for the new KF
00213         new_kf_info,   // Output info
00214         true           // Also run local optimization?
00215         );
00216 
00217     cout << "Created KF #" << new_kf_info.kf_id
00218         << " | # kf-to-kf edges created:" <<  new_kf_info.created_edge_ids.size() << endl
00219         << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl
00220         << "-------------------------------------------------------" << endl;
00221 
00222 
00223     // Dump the relative pose of KF#0 wrt KF#1:
00224     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;
00225 
00226     // --------------------------------------------------------------------------------
00227     // Saving RBA graph as a DOT file:
00228     // --------------------------------------------------------------------------------
00229     const string sFil = "graph.dot";
00230     cout << "Saving final graph of KFs and LMs to: " << sFil << endl;
00231     rba.save_graph_as_dot(sFil, true /* LMs=save */);
00232     cout << "Done.\n";
00233 
00234     // --------------------------------------------------------------------------------
00235     // Show 3D view of the resulting map:
00236     // --------------------------------------------------------------------------------
00237     my_srba_t::TOpenGLRepresentationOptions  opengl_options;
00238     mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create();
00239 
00240     rba.build_opengl_representation(
00241         0,  // Root KF,
00242         opengl_options, // Rendering options
00243         rba_3d  // Output scene
00244         );
00245 
00246     // Display:
00247 #if MRPT_HAS_WXWIDGETS
00248     mrpt::gui::CDisplayWindow3D win("RBA results",640,480);
00249     {
00250         mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
00251         scene->insert(rba_3d);
00252         win.unlockAccess3DScene();
00253     }
00254     win.setCameraZoom( 4 );
00255     win.repaint();
00256 
00257     cout << "Press any key or close window to exit.\n";
00258     win.waitForKey();
00259 #endif
00260 
00261     return 0; // All ok
00262 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends