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 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