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 #include <mrpt/graphslam.h> // For global map recovery only 00017 #include <mrpt/opengl/graph_tools.h> // To render the global map 00018 00019 using namespace srba; 00020 using namespace std; 00021 using mrpt::utils::DEG2RAD; 00022 using mrpt::utils::square; 00023 00024 00025 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT 00026 { 00027 // typedef ecps::local_areas_fixed_size edge_creation_policy_t; //!< One of the most important choices: how to construct the relative coordinates graph problem 00028 // typedef options::sensor_pose_on_robot_none sensor_pose_on_robot_t; //!< The sensor pose coincides with the robot pose 00029 typedef options::observation_noise_constant_matrix<observations::RelativePoses_3D> obs_noise_matrix_t; // The sensor noise matrix is the same for all observations and equal to some given matrix 00030 // typedef options::solver_LM_schur_dense_cholesky solver_t; //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky) 00031 }; 00032 00033 typedef RbaEngine< 00034 kf2kf_poses::SE3, // Parameterization of KF-to-KF poses 00035 landmarks::RelativePoses3D, // Parameterization of landmark positions 00036 observations::RelativePoses_3D, // Type of observations 00037 RBA_OPTIONS 00038 > my_srba_t; 00039 00040 // -------------------------------------------------------------------------------- 00041 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit ) 00042 // -------------------------------------------------------------------------------- 00043 const double STD_NOISE_XYZ = 0.05; 00044 const double STD_NOISE_ANGLES = mrpt::utils::DEG2RAD(1.0); 00045 00046 struct basic_graph_slam_dataset_entry_t 00047 { 00048 unsigned int current_kf; 00049 unsigned int observed_kf; 00050 double x,y,z, yaw,pitch,roll, qr,qx,qy,qz; // Relative pose of "observed_kf" as seen from "current_kf" 00051 }; 00052 basic_graph_slam_dataset_entry_t dataset[] = { 00053 { 1, 0, -1.78055512, 1.11331694, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00054 { 2, 1, -1.71545942, 2.05914961, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00055 { 2, 0, -2.96619160, 3.74601658, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00056 { 3, 2, -1.15014065, 2.45631509, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00057 { 4, 3, -0.71839088, 2.17858845, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00058 { 4, 2, -0.89163376, 4.88530127, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00059 { 5, 4, -1.33870852, 1.73631597, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00060 { 5, 3, -1.21151269, 4.02676447, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00061 { 6, 5, -1.67977719, 2.03565806, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00062 { 6, 4, -2.29159821, 4.14103420, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00063 { 7, 6, -1.49006905, 2.30876608, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00064 { 8, 7, -1.15992524, 2.21845386, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00065 { 9, 8, -1.28889269, 1.78614744, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00066 { 9, 7, -1.55814427, 4.27501619, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00067 { 10, 9, -1.67026750, 1.96210498, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00068 { 10, 8, -2.21751078, 4.09566815, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00069 { 11, 10, -1.55210516, 2.27651848, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00070 { 12, 11, -1.21625554, 2.27164636, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00071 { 13, 12, -1.45455725, 1.51179033, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00072 { 13, 11, -2.13482825, 3.99712454, 0.00000000, -0.59839931, -0.00000000, 0.00000000, 0.95557270, 0.00000000, -0.00000000, -0.29475552}, 00073 { 14, 13, -2.36655195, 0.41536284, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00074 { 14, 12, -3.82110920, 1.92715317, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00075 { 15, 14, -2.74448431, -0.11373775, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00076 { 15, 0, 4.16910212, 0.67638546, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00077 { 16, 0, 1.58658626, 0.30349575, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00078 { 16, 15, -2.58251586, -0.37288971, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00079 { 16, 1, 1.97243380, 2.36770815, 0.00000000, 1.94479552, -0.00000000, 0.00000000, 0.56332003, 0.00000000, 0.00000000, 0.82623879}, 00080 }; 00081 00082 int main(int argc, char**argv) 00083 { 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; 00092 //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) 00093 00094 // Information matrix for relative pose observations: 00095 { 00096 Eigen::Matrix<double,6,6> ObsL; 00097 ObsL.setZero(); 00098 // X,Y,Z: 00099 for (int i=0;i<3;i++) ObsL(i,i) = 1/square(STD_NOISE_XYZ); 00100 // Yaw,pitch,roll: 00101 for (int i=0;i<3;i++) ObsL(3+i,3+i) = 1/square(STD_NOISE_ANGLES); 00102 00103 // Set: 00104 rba.parameters.obs_noise.lambda = ObsL; 00105 } 00106 00107 // =========== Topology parameters =========== 00108 rba.parameters.srba.max_tree_depth = 3; 00109 rba.parameters.srba.max_optimize_depth = 3; 00110 rba.parameters.ecp.submap_size = 5; 00111 rba.parameters.ecp.min_obs_to_loop_closure = 1; 00112 // =========================================== 00113 00114 // -------------------------------------------------------------------------------- 00115 // Dump parameters to console (for checking/debugging only) 00116 // -------------------------------------------------------------------------------- 00117 cout << "RBA parameters:\n-----------------\n"; 00118 rba.parameters.srba.dumpToConsole(); 00119 00120 #if MRPT_HAS_WXWIDGETS 00121 mrpt::gui::CDisplayWindow3D win("RBA results",640,480); 00122 #endif 00123 00124 // -------------------------------------------------------------------------------- 00125 // Process the dataset: 00126 // -------------------------------------------------------------------------------- 00127 const size_t nObs = sizeof(dataset)/sizeof(dataset[0]); 00128 size_t cur_kf = 0; // Start at keyframe #0 in the dataset 00129 00130 for (size_t obsIdx = 0; obsIdx<nObs; cur_kf++ /* move to next KF */ ) 00131 { 00132 // Create list of observations for keyframe: "cur_kf" 00133 my_srba_t::new_kf_observations_t list_obs; 00134 00135 // To emulate graph-SLAM, each keyframe MUST have exactly ONE fixed "fake landmark", representing its pose: 00136 // ------------------------------------------------------------------------------------------------------------ 00137 { 00138 my_srba_t::new_kf_observation_t obs_field; 00139 obs_field.is_fixed = true; 00140 obs_field.obs.feat_id = cur_kf; // Feature ID == keyframe ID 00141 obs_field.obs.obs_data.x = 0; // Landmark values are actually ignored. 00142 obs_field.obs.obs_data.y = 0; 00143 obs_field.obs.obs_data.z = 0; 00144 obs_field.obs.obs_data.yaw = 0; 00145 obs_field.obs.obs_data.pitch = 0; 00146 obs_field.obs.obs_data.roll = 0; 00147 list_obs.push_back( obs_field ); 00148 } 00149 00150 // The rest "observations" are real observations of relative poses: 00151 // ----------------------------------------------------------------- 00152 while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs ) 00153 { 00154 my_srba_t::new_kf_observation_t obs_field; 00155 obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated) 00156 obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value. 00157 00158 obs_field.obs.feat_id = dataset[obsIdx].observed_kf; 00159 obs_field.obs.obs_data.x = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00160 obs_field.obs.obs_data.y = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00161 obs_field.obs.obs_data.z = dataset[obsIdx].z + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00162 obs_field.obs.obs_data.yaw = dataset[obsIdx].yaw + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00163 obs_field.obs.obs_data.pitch = dataset[obsIdx].pitch + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00164 obs_field.obs.obs_data.roll = dataset[obsIdx].roll + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00165 00166 list_obs.push_back( obs_field ); 00167 obsIdx++; // Next dataset entry 00168 } 00169 00170 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00171 // ============================================================================================ 00172 my_srba_t::TNewKeyFrameInfo new_kf_info; 00173 rba.define_new_keyframe( 00174 list_obs, // Input observations for the new KF 00175 new_kf_info, // Output info 00176 true // Also run local optimization? 00177 ); 00178 00179 cout << "Created KF #" << new_kf_info.kf_id 00180 << " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl 00181 << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl 00182 << "-------------------------------------------------------" << endl; 00183 00184 // Display: 00185 #if MRPT_HAS_WXWIDGETS 00186 // -------------------------------------------------------------------------------- 00187 // Show 3D view of the resulting map: 00188 // -------------------------------------------------------------------------------- 00189 my_srba_t::TOpenGLRepresentationOptions opengl_options; 00190 mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create(); 00191 00192 rba.build_opengl_representation( 00193 new_kf_info.kf_id , // Root KF: the current (latest) KF 00194 opengl_options, // Rendering options 00195 rba_3d // Output scene 00196 ); 00197 00198 { 00199 mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); 00200 scene->clear(); 00201 scene->insert(rba_3d); 00202 win.unlockAccess3DScene(); 00203 } 00204 win.repaint(); 00205 00206 cout << "Press any key to continue.\n"; 00207 win.waitForKey(); 00208 #endif 00209 00210 } // end-for each dataset entry 00211 00212 00213 // -------------------------------------------------------------------------------- 00214 // Get equivalent graph-slam problem: 00215 // -------------------------------------------------------------------------------- 00216 mrpt::graphs::CNetworkOfPoses3D poseGraph; 00217 rba.get_global_graphslam_problem(poseGraph); 00218 00219 // Run optimization: 00220 mrpt::graphslam::TResultInfoSpaLevMarq out_info; 00221 mrpt::utils::TParametersDouble extra_params; 00222 00223 mrpt::graphslam::optimize_graph_spa_levmarq( 00224 poseGraph, 00225 out_info, 00226 NULL, /* in_nodes_to_optimize, NULL=all */ 00227 extra_params 00228 ); 00229 00230 // Render resulting graph: 00231 #if MRPT_HAS_WXWIDGETS 00232 mrpt::gui::CDisplayWindow3D win2("Global optimized map",640,480); 00233 { 00234 mrpt::opengl::COpenGLScenePtr &scene = win2.get3DSceneAndLock(); 00235 00236 mrpt::utils::TParametersDouble render_params; // See docs for mrpt::opengl::graph_tools::graph_visualize() 00237 render_params["show_ID_labels"] = 1; 00238 00239 // Get opengl representation of the graph: 00240 mrpt::opengl::CSetOfObjectsPtr gl_global_map = mrpt::opengl::graph_tools::graph_visualize( poseGraph,render_params ); 00241 scene->insert(gl_global_map); 00242 00243 win2.unlockAccess3DScene(); 00244 win2.repaint(); 00245 mrpt::system::pause(); 00246 } 00247 00248 00249 #endif 00250 00251 00252 00253 return 0; // All ok 00254 }