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 mrpt::utils; 00018 using namespace std; 00019 using mrpt::utils::DEG2RAD; 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 basic_graph_slam_dataset_entry_t dataset[] = { 00049 { 1, 0, -1.78055512, 1.11331694, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00050 { 2, 1, -1.71545942, 2.05914961, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00051 { 2, 0, -2.96619160, 3.74601658, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00052 { 3, 2, -1.15014065, 2.45631509, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00053 { 4, 3, -0.71839088, 2.17858845, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00054 { 4, 2, -0.89163376, 4.88530127, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00055 { 5, 4, -1.33870852, 1.73631597, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00056 { 5, 3, -1.21151269, 4.02676447, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00057 { 6, 5, -1.67977719, 2.03565806, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00058 { 6, 4, -2.29159821, 4.14103420, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00059 { 7, 6, -1.49006905, 2.30876608, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00060 { 8, 7, -1.15992524, 2.21845386, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00061 { 9, 8, -1.28889269, 1.78614744, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00062 { 9, 7, -1.55814427, 4.27501619, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00063 { 10, 9, -1.67026750, 1.96210498, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00064 { 10, 8, -2.21751078, 4.09566815, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00065 { 11, 10, -1.55210516, 2.27651848, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00066 { 12, 11, -1.21625554, 2.27164636, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00067 { 13, 12, -1.45455725, 1.51179033, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00068 { 13, 11, -2.13482825, 3.99712454, 0.00000000, -0.59839931, -0.00000000, 0.00000000, 0.95557270, 0.00000000, -0.00000000, -0.29475552}, 00069 { 14, 13, -2.36655195, 0.41536284, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00070 { 14, 12, -3.82110920, 1.92715317, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00071 { 15, 14, -2.74448431, -0.11373775, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00072 { 15, 0, 4.16910212, 0.67638546, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00073 { 16, 0, 1.58658626, 0.30349575, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00074 { 16, 15, -2.58251586, -0.37288971, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00075 { 16, 1, 1.97243380, 2.36770815, 0.00000000, 1.94479552, -0.00000000, 0.00000000, 0.56332003, 0.00000000, 0.00000000, 0.82623879}, 00076 }; 00077 00078 int main(int argc, char**argv) 00079 { 00080 my_srba_t rba; // Create an empty RBA problem 00081 00082 // -------------------------------------------------------------------------------- 00083 // Set parameters 00084 // -------------------------------------------------------------------------------- 00085 rba.setVerbosityLevel( 1 ); // 0: None; 1:Important only; 2:Verbose 00086 00087 rba.parameters.srba.use_robust_kernel = false; 00088 //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) 00089 00090 // Information matrix for relative pose observations: 00091 { 00092 Eigen::Matrix3d ObsL; 00093 ObsL.setZero(); 00094 ObsL(0,0) = 1/square(STD_NOISE_XY); // x 00095 ObsL(1,1) = 1/square(STD_NOISE_XY); // y 00096 ObsL(2,2) = 1/square(STD_NOISE_YAW); // phi 00097 00098 // Set: 00099 rba.parameters.obs_noise.lambda = ObsL; 00100 } 00101 00102 // =========== Topology parameters =========== 00103 rba.parameters.srba.max_tree_depth = 3; 00104 rba.parameters.srba.max_optimize_depth = 3; 00105 rba.parameters.ecp.submap_size = 5; 00106 rba.parameters.ecp.min_obs_to_loop_closure = 1; 00107 // =========================================== 00108 00109 // -------------------------------------------------------------------------------- 00110 // Dump parameters to console (for checking/debugging only) 00111 // -------------------------------------------------------------------------------- 00112 cout << "RBA parameters:\n-----------------\n"; 00113 rba.parameters.srba.dumpToConsole(); 00114 00115 #if MRPT_HAS_WXWIDGETS 00116 mrpt::gui::CDisplayWindow3D win("RBA results",640,480); 00117 #endif 00118 00119 // -------------------------------------------------------------------------------- 00120 // Process the dataset: 00121 // -------------------------------------------------------------------------------- 00122 const size_t nObs = sizeof(dataset)/sizeof(dataset[0]); 00123 size_t cur_kf = 0; // Start at keyframe #0 in the dataset 00124 00125 for (size_t obsIdx = 0; obsIdx<nObs; cur_kf++ /* move to next KF */ ) 00126 { 00127 // Create list of observations for keyframe: "cur_kf" 00128 my_srba_t::new_kf_observations_t list_obs; 00129 00130 // To emulate graph-SLAM, each keyframe MUST have exactly ONE fixed "fake landmark", representing its pose: 00131 // ------------------------------------------------------------------------------------------------------------ 00132 { 00133 my_srba_t::new_kf_observation_t obs_field; 00134 obs_field.is_fixed = true; 00135 obs_field.obs.feat_id = cur_kf; // Feature ID == keyframe ID 00136 obs_field.obs.obs_data.x = 0; // Landmark values are actually ignored. 00137 obs_field.obs.obs_data.y = 0; 00138 obs_field.obs.obs_data.yaw = 0; 00139 list_obs.push_back( obs_field ); 00140 } 00141 00142 // The rest "observations" are real observations of relative poses: 00143 // ----------------------------------------------------------------- 00144 while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs ) 00145 { 00146 my_srba_t::new_kf_observation_t obs_field; 00147 obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated) 00148 obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value. 00149 00150 obs_field.obs.feat_id = dataset[obsIdx].observed_kf; 00151 obs_field.obs.obs_data.x = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY); 00152 obs_field.obs.obs_data.y = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XY); 00153 obs_field.obs.obs_data.yaw = dataset[obsIdx].yaw + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_YAW); 00154 00155 list_obs.push_back( obs_field ); 00156 obsIdx++; // Next dataset entry 00157 } 00158 00159 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00160 // ============================================================================================ 00161 my_srba_t::TNewKeyFrameInfo new_kf_info; 00162 rba.define_new_keyframe( 00163 list_obs, // Input observations for the new KF 00164 new_kf_info, // Output info 00165 true // Also run local optimization? 00166 ); 00167 00168 cout << "Created KF #" << new_kf_info.kf_id 00169 << " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl 00170 << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl 00171 << "-------------------------------------------------------" << endl; 00172 00173 // Display: 00174 #if MRPT_HAS_WXWIDGETS 00175 // -------------------------------------------------------------------------------- 00176 // Show 3D view of the resulting map: 00177 // -------------------------------------------------------------------------------- 00178 my_srba_t::TOpenGLRepresentationOptions opengl_options; 00179 mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create(); 00180 00181 rba.build_opengl_representation( 00182 new_kf_info.kf_id , // Root KF: the current (latest) KF 00183 opengl_options, // Rendering options 00184 rba_3d // Output scene 00185 ); 00186 00187 { 00188 mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); 00189 scene->clear(); 00190 scene->insert(rba_3d); 00191 win.unlockAccess3DScene(); 00192 } 00193 win.repaint(); 00194 00195 cout << "Press any key to continue.\n"; 00196 win.waitForKey(); 00197 #endif 00198 00199 } // end-for each dataset entry 00200 00201 00202 // -------------------------------------------------------------------------------- 00203 // Saving RBA graph as a DOT file: 00204 // -------------------------------------------------------------------------------- 00205 const string sFil = "graph.dot"; 00206 cout << "Saving final graph of KFs and LMs to: " << sFil << endl; 00207 rba.save_graph_as_dot(sFil, true /* LMs=save */); 00208 cout << "Done.\n"; 00209 00210 00211 return 0; // All ok 00212 }