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::square; 00019 00020 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT 00021 { 00022 // typedef ecps::local_areas_fixed_size edge_creation_policy_t; //!< One of the most important choices: how to construct the relative coordinates graph problem 00023 // typedef options::sensor_pose_on_robot_none sensor_pose_on_robot_t; //!< The sensor pose coincides with the robot pose 00024 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 00025 // typedef options::solver_LM_schur_dense_cholesky solver_t; //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky) 00026 }; 00027 00028 typedef RbaEngine< 00029 kf2kf_poses::SE3, // Parameterization of KF-to-KF poses 00030 landmarks::RelativePoses3D, // Parameterization of landmark positions 00031 observations::RelativePoses_3D, // Type of observations 00032 RBA_OPTIONS 00033 > my_srba_t; 00034 00035 // -------------------------------------------------------------------------------- 00036 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit ) 00037 // -------------------------------------------------------------------------------- 00038 const double STD_NOISE_XYZ = 0.01; 00039 const double STD_NOISE_ANGLES = mrpt::utils::DEG2RAD(0.5); 00040 00041 struct basic_graph_slam_dataset_entry_t 00042 { 00043 unsigned int current_kf; 00044 unsigned int observed_kf; 00045 double x,y,z, yaw,pitch,roll, qr,qx,qy,qz; // Relative pose of "observed_kf" as seen from "current_kf" 00046 }; 00047 basic_graph_slam_dataset_entry_t dataset[] = { 00048 { 1, 0, -1.78055512, 1.11331694, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00049 { 2, 1, -1.71545942, 2.05914961, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00050 { 2, 0, -2.96619160, 3.74601658, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00051 { 3, 2, -1.15014065, 2.45631509, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00052 { 4, 3, -0.71839088, 2.17858845, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00053 { 4, 2, -0.89163376, 4.88530127, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00054 { 5, 4, -1.33870852, 1.73631597, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00055 { 5, 3, -1.21151269, 4.02676447, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00056 { 6, 5, -1.67977719, 2.03565806, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00057 { 6, 4, -2.29159821, 4.14103420, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00058 { 7, 6, -1.49006905, 2.30876608, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00059 { 8, 7, -1.15992524, 2.21845386, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00060 { 9, 8, -1.28889269, 1.78614744, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00061 { 9, 7, -1.55814427, 4.27501619, 0.00000000, -0.74799802, -0.00000000, 0.00000000, 0.93087379, 0.00000000, -0.00000000, -0.36534092}, 00062 { 10, 9, -1.67026750, 1.96210498, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00063 { 10, 8, -2.21751078, 4.09566815, 0.00000000, -0.74799839, -0.00000000, 0.00000000, 0.93087372, 0.00000000, -0.00000000, -0.36534109}, 00064 { 11, 10, -1.55210516, 2.27651848, 0.00000000, -0.37399882, -0.00000000, 0.00000000, 0.98256650, 0.00000000, -0.00000000, -0.18591146}, 00065 { 12, 11, -1.21625554, 2.27164636, 0.00000000, -0.37399920, -0.00000000, 0.00000000, 0.98256647, 0.00000000, -0.00000000, -0.18591164}, 00066 { 13, 12, -1.45455725, 1.51179033, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00067 { 13, 11, -2.13482825, 3.99712454, 0.00000000, -0.59839931, -0.00000000, 0.00000000, 0.95557270, 0.00000000, -0.00000000, -0.29475552}, 00068 { 14, 13, -2.36655195, 0.41536284, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00069 { 14, 12, -3.82110920, 1.92715317, 0.00000000, -0.22440012, -0.00000000, 0.00000000, 0.99371217, 0.00000000, -0.00000000, -0.11196480}, 00070 { 15, 14, -2.74448431, -0.11373775, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00071 { 15, 0, 4.16910212, 0.67638546, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00072 { 16, 0, 1.58658626, 0.30349575, 0.00000000, 1.57079633, -0.00000000, 0.00000000, 0.70710678, 0.00000000, 0.00000000, 0.70710678}, 00073 { 16, 15, -2.58251586, -0.37288971, 0.00000000, 0.00000000, -0.00000000, 0.00000000, 1.00000000, 0.00000000, 0.00000000, 0.00000000}, 00074 { 16, 1, 1.97243380, 2.36770815, 0.00000000, 1.94479552, -0.00000000, 0.00000000, 0.56332003, 0.00000000, 0.00000000, 0.82623879}, 00075 }; 00076 00077 int main(int argc, char**argv) 00078 { 00079 my_srba_t rba; // Create an empty RBA problem 00080 00081 // -------------------------------------------------------------------------------- 00082 // Set parameters 00083 // -------------------------------------------------------------------------------- 00084 rba.setVerbosityLevel( 1 ); // 0: None; 1:Important only; 2:Verbose 00085 00086 rba.parameters.srba.use_robust_kernel = false; 00087 //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) 00088 00089 // Information matrix for relative pose observations: 00090 { 00091 Eigen::Matrix<double,6,6> ObsL; 00092 ObsL.setZero(); 00093 // X,Y,Z: 00094 for (int i=0;i<3;i++) ObsL(i,i) = 1/square(STD_NOISE_XYZ); 00095 // Yaw,pitch,roll: 00096 for (int i=0;i<3;i++) ObsL(3+i,3+i) = 1/square(STD_NOISE_ANGLES); 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.z = 0; 00139 obs_field.obs.obs_data.yaw = 0; 00140 obs_field.obs.obs_data.pitch = 0; 00141 obs_field.obs.obs_data.roll = 0; 00142 list_obs.push_back( obs_field ); 00143 } 00144 00145 // The rest "observations" are real observations of relative poses: 00146 // ----------------------------------------------------------------- 00147 while ( dataset[obsIdx].current_kf == cur_kf && obsIdx<nObs ) 00148 { 00149 my_srba_t::new_kf_observation_t obs_field; 00150 obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated) 00151 obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value. 00152 00153 obs_field.obs.feat_id = dataset[obsIdx].observed_kf; 00154 obs_field.obs.obs_data.x = dataset[obsIdx].x + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00155 obs_field.obs.obs_data.y = dataset[obsIdx].y + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00156 obs_field.obs.obs_data.z = dataset[obsIdx].z + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_XYZ); 00157 obs_field.obs.obs_data.yaw = dataset[obsIdx].yaw + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00158 obs_field.obs.obs_data.pitch = dataset[obsIdx].pitch + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00159 obs_field.obs.obs_data.roll = dataset[obsIdx].roll + mrpt::random::randomGenerator.drawGaussian1D(0,STD_NOISE_ANGLES); 00160 00161 list_obs.push_back( obs_field ); 00162 obsIdx++; // Next dataset entry 00163 } 00164 00165 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00166 // ============================================================================================ 00167 my_srba_t::TNewKeyFrameInfo new_kf_info; 00168 rba.define_new_keyframe( 00169 list_obs, // Input observations for the new KF 00170 new_kf_info, // Output info 00171 true // Also run local optimization? 00172 ); 00173 00174 cout << "Created KF #" << new_kf_info.kf_id 00175 << " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl 00176 << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl 00177 << "-------------------------------------------------------" << endl; 00178 00179 // Display: 00180 #if MRPT_HAS_WXWIDGETS 00181 // -------------------------------------------------------------------------------- 00182 // Show 3D view of the resulting map: 00183 // -------------------------------------------------------------------------------- 00184 my_srba_t::TOpenGLRepresentationOptions opengl_options; 00185 mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create(); 00186 00187 rba.build_opengl_representation( 00188 new_kf_info.kf_id , // Root KF: the current (latest) KF 00189 opengl_options, // Rendering options 00190 rba_3d // Output scene 00191 ); 00192 00193 { 00194 mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); 00195 scene->clear(); 00196 scene->insert(rba_3d); 00197 win.unlockAccess3DScene(); 00198 } 00199 win.repaint(); 00200 00201 cout << "Press any key to continue.\n"; 00202 win.waitForKey(); 00203 #endif 00204 00205 } // end-for each dataset entry 00206 00207 00208 // -------------------------------------------------------------------------------- 00209 // Saving RBA graph as a DOT file: 00210 // -------------------------------------------------------------------------------- 00211 const string sFil = "graph.dot"; 00212 cout << "Saving final graph of KFs and LMs to: " << sFil << endl; 00213 rba.save_graph_as_dot(sFil, true /* LMs=save */); 00214 cout << "Done.\n"; 00215 00216 00217 return 0; // All ok 00218 }