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 //#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 using namespace srba; 00017 using namespace std; 00018 using mrpt::utils::DEG2RAD; 00019 using mrpt::utils::square; 00020 00021 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT 00022 { 00023 // typedef ecps::local_areas_fixed_size edge_creation_policy_t; //!< One of the most important choices: how to construct the relative coordinates graph problem 00024 // typedef options::sensor_pose_on_robot_none sensor_pose_on_robot_t; //!< The sensor pose coincides with the robot pose 00025 typedef options::observation_noise_constant_matrix<observations::RelativePoses_2D> obs_noise_matrix_t; // The sensor noise matrix is the same for all observations and equal to some given matrix 00026 // typedef options::solver_LM_schur_dense_cholesky solver_t; //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky) 00027 }; 00028 00029 typedef RbaEngine< 00030 kf2kf_poses::SE2, // Parameterization of KF-to-KF poses 00031 landmarks::RelativePoses2D, // Parameterization of landmark positions 00032 observations::RelativePoses_2D, // Type of observations 00033 RBA_OPTIONS 00034 > my_srba_t; 00035 00036 // -------------------------------------------------------------------------------- 00037 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit ) 00038 // -------------------------------------------------------------------------------- 00039 const double STD_NOISE_XY = 0.001; 00040 const double STD_NOISE_YAW = DEG2RAD(0.05); 00041 00042 struct basic_graph_slam_dataset_entry_t 00043 { 00044 unsigned int current_kf; 00045 unsigned int observed_kf; 00046 double x,y,z, yaw,pitch,roll, qr,qx,qy,qz; // Relative pose of "observed_kf" as seen from "current_kf" 00047 }; 00048 00049 // Corrupt observation test: add it, later remove (e.g. when it's clear it's an outlier): 00050 const size_t obs_index_to_remove = 2; 00051 const size_t kf_at_which_do_remove = 8; 00052 00053 basic_graph_slam_dataset_entry_t dataset[] = { 00054 { 1, 0, -1.78055512, 1.11331694, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00055 { 2, 1, -1.71545942, 2.05914961, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00056 //--- 00057 //Original: { 2, 0, -2.96619160, 3.74601658, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00058 //Corrupt observation: 00059 { 2, 0, +8.00000000, -17.50000000, 0.00000000, +0.70000000, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00060 //--- 00061 { 3, 2, -1.15014065, 2.45631509, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00062 { 4, 3, -0.71839088, 2.17858845, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00063 { 4, 2, -0.89163376, 4.88530127, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00064 { 5, 4, -1.33870852, 1.73631597, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00065 { 5, 3, -1.21151269, 4.02676447, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00066 { 6, 5, -1.67977719, 2.03565806, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00067 { 6, 4, -2.29159821, 4.14103420, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00068 { 7, 6, -1.49006905, 2.30876608, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00069 { 8, 7, -1.15992524, 2.21845386, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00070 { 9, 8, -1.28889269, 1.78614744, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00071 { 9, 7, -1.55814427, 4.27501619, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00072 { 10, 9, -1.67026750, 1.96210498, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00073 { 10, 8, -2.21751078, 4.09566815, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00074 { 11, 10, -1.55210516, 2.27651848, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00075 { 12, 11, -1.21625554, 2.27164636, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00076 { 13, 12, -1.45455725, 1.51179033, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00077 { 13, 11, -2.13482825, 3.99712454, 0.00000000, -0.59839931, -0.00000000, 0.00000000, 0.95557270, 0.00000000, -0.00000000, -0.29475552}, 00078 { 14, 13, -2.36655195, 0.41536284, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00079 { 14, 12, -3.82110920, 1.92715317, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00080 { 15, 14, -2.74448431, -0.11373775, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00081 { 15, 0, 4.16910212, 0.67638546, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00082 { 16, 0, 1.58658626, 0.30349575, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00083 { 16, 15, -2.58251586, -0.37288971, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00084 { 16, 1, 1.97243380, 2.36770815, 0.00000000, 1.94479552, -0.00000000, 0.00000000, 0.56332003, 0.00000000, 0.00000000, 0.82623879}, 00085 }; 00086 00087 int main(int argc, char**argv) 00088 { 00089 my_srba_t rba; // Create an empty RBA problem 00090 00091 // -------------------------------------------------------------------------------- 00092 // Set parameters 00093 // -------------------------------------------------------------------------------- 00094 rba.setVerbosityLevel( 1 ); // 0: None; 1:Important only; 2:Verbose 00095 00096 rba.parameters.srba.use_robust_kernel = false; 00097 //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) 00098 00099 // Information matrix for relative pose observations: 00100 { 00101 Eigen::Matrix3d ObsL; 00102 ObsL.setZero(); 00103 ObsL(0,0) = 1/square(STD_NOISE_XY); // x 00104 ObsL(1,1) = 1/square(STD_NOISE_XY); // y 00105 ObsL(2,2) = 1/square(STD_NOISE_YAW); // phi 00106 00107 // Set: 00108 rba.parameters.obs_noise.lambda = ObsL; 00109 } 00110 00111 // =========== Topology parameters =========== 00112 rba.parameters.srba.max_tree_depth = 3; 00113 rba.parameters.srba.max_optimize_depth = 3; 00114 rba.parameters.ecp.submap_size = 5; 00115 rba.parameters.ecp.min_obs_to_loop_closure = 1; 00116 // =========================================== 00117 00118 // -------------------------------------------------------------------------------- 00119 // Dump parameters to console (for checking/debugging only) 00120 // -------------------------------------------------------------------------------- 00121 cout << "RBA parameters:\n-----------------\n"; 00122 rba.parameters.srba.dumpToConsole(); 00123 00124 #if MRPT_HAS_WXWIDGETS 00125 mrpt::gui::CDisplayWindow3D win("RBA results",640,480); 00126 #endif 00127 00128 // -------------------------------------------------------------------------------- 00129 // Process the dataset: 00130 // -------------------------------------------------------------------------------- 00131 const size_t nObs = sizeof(dataset)/sizeof(dataset[0]); 00132 size_t cur_kf = 0; // Start at keyframe #0 in the dataset 00133 00134 for (size_t obsIdx = 0; obsIdx<nObs; cur_kf++ /* move to next KF */ ) 00135 { 00136 // Create list of observations for keyframe: "cur_kf" 00137 my_srba_t::new_kf_observations_t list_obs; 00138 00139 // To emulate graph-SLAM, each keyframe MUST have exactly ONE fixed "fake landmark", representing its pose: 00140 // ------------------------------------------------------------------------------------------------------------ 00141 { 00142 my_srba_t::new_kf_observation_t obs_field; 00143 obs_field.is_fixed = true; 00144 obs_field.obs.feat_id = cur_kf; // Feature ID == keyframe ID 00145 obs_field.obs.obs_data.x = 0; // Landmark values are actually ignored. 00146 obs_field.obs.obs_data.y = 0; 00147 obs_field.obs.obs_data.yaw = 0; 00148 list_obs.push_back( obs_field ); 00149 } 00150 00151 // The rest "observations" are real observations of relative poses: 00152 // ----------------------------------------------------------------- 00153 while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs ) 00154 { 00155 my_srba_t::new_kf_observation_t obs_field; 00156 obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated) 00157 obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value. 00158 00159 obs_field.obs.feat_id = dataset[obsIdx].observed_kf; 00160 obs_field.obs.obs_data.x = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY); 00161 obs_field.obs.obs_data.y = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY); 00162 obs_field.obs.obs_data.yaw = dataset[obsIdx].yaw + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_YAW); 00163 00164 list_obs.push_back( obs_field ); 00165 obsIdx++; // Next dataset entry 00166 } 00167 00168 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00169 // ============================================================================================ 00170 my_srba_t::TNewKeyFrameInfo new_kf_info; 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 // Remove observation test: 00183 if (cur_kf==kf_at_which_do_remove) 00184 { 00185 MRPT_TODO("XXX") 00186 // obs_index_to_remove = 2; 00187 } 00188 00189 00190 // Display: 00191 #if MRPT_HAS_WXWIDGETS 00192 // -------------------------------------------------------------------------------- 00193 // Show 3D view of the resulting map: 00194 // -------------------------------------------------------------------------------- 00195 my_srba_t::TOpenGLRepresentationOptions opengl_options; 00196 mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create(); 00197 00198 rba.build_opengl_representation( 00199 new_kf_info.kf_id , // Root KF: the current (latest) KF 00200 opengl_options, // Rendering options 00201 rba_3d // Output scene 00202 ); 00203 00204 { 00205 mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); 00206 scene->clear(); 00207 scene->insert(rba_3d); 00208 win.unlockAccess3DScene(); 00209 } 00210 win.repaint(); 00211 00212 cout << "Press any key to continue.\n"; 00213 win.waitForKey(); 00214 #endif 00215 00216 } // end-for each dataset entry 00217 00218 00219 // -------------------------------------------------------------------------------- 00220 // Saving RBA graph as a DOT file: 00221 // -------------------------------------------------------------------------------- 00222 const string sFil = "graph.dot"; 00223 cout << "Saving final graph of KFs and LMs to: " << sFil << endl; 00224 rba.save_graph_as_dot(sFil, true /* LMs=save */); 00225 cout << "Done.\n"; 00226 00227 00228 return 0; // All ok 00229 }