SRBA: Sparser Relative Bundle Adjustment
|
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 }