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