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 00013 using namespace srba; 00014 using namespace std; 00015 using mrpt::utils::DEG2RAD; 00016 00017 struct RBA_OPTIONS : public RBA_OPTIONS_DEFAULT 00018 { 00019 // typedef ecps::local_areas_fixed_size edge_creation_policy_t; //!< One of the most important choices: how to construct the relative coordinates graph problem 00020 typedef options::sensor_pose_on_robot_se3 sensor_pose_on_robot_t; 00021 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) 00022 // typedef options::solver_LM_schur_dense_cholesky solver_t; //!< Solver algorithm (Default: Lev-Marq, with Schur, with dense Cholesky) 00023 }; 00024 00025 typedef RbaEngine< 00026 kf2kf_poses::SE2, // Parameterization of KF-to-KF poses 00027 landmarks::Euclidean3D, // Parameterization of landmark positions 00028 observations::StereoCamera, // Type of observations 00029 RBA_OPTIONS 00030 > my_srba_t; 00031 00032 // -------------------------------------------------------------------------------- 00033 // A test dataset (generated with https://github.com/jlblancoc/recursive-world-toolkit ) 00034 // -------------------------------------------------------------------------------- 00035 struct basic_stereo_dataset_entry_t 00036 { 00037 unsigned int landmark_id; 00038 double l_px_x, l_px_y, r_px_x, r_px_y; 00039 }; 00040 // Observations for KF#0 00041 basic_stereo_dataset_entry_t dataset0[] = { 00042 { 39, 539.29449463, 384.00000000, 524.23114014, 384.00000000}, 00043 { 35, 515.28930664, 384.00000000, 501.31469727, 384.00000000}, 00044 { 20, 141.98646545, 384.00000000, 113.16947174, 384.00000000}, 00045 { 28, 549.01715088, 384.00000000, 536.19653320, 384.00000000}, 00046 { 18, 848.99078369, 384.00000000, 826.60614014, 384.00000000}, 00047 { 37, 599.72821045, 384.00000000, 587.27532959, 384.00000000}, 00048 { 6, 601.74615479, 384.00000000, 590.90521240, 384.00000000}, 00049 { 13, 485.88748169, 384.00000000, 476.89270020, 384.00000000}, 00050 { 30, 991.59832764, 384.00000000, 969.21966553, 384.00000000}, 00051 { 24, 437.52539063, 384.00000000, 428.89263916, 384.00000000}, 00052 { 3, 580.46923828, 384.00000000, 571.96411133, 384.00000000}, 00053 { 26, 562.13531494, 384.00000000, 553.93872070, 384.00000000}, 00054 { 8, 681.00463867, 384.00000000, 670.68438721, 384.00000000}, 00055 { 1, 695.37768555, 384.00000000, 684.69763184, 384.00000000}, 00056 { 32, 572.87854004, 384.00000000, 564.88854980, 384.00000000}, 00057 { 11, 721.99755859, 384.00000000, 711.39343262, 384.00000000}, 00058 { 12, 470.31097412, 384.00000000, 463.17254639, 384.00000000}, 00059 { 5, 554.08819580, 384.00000000, 547.04064941, 384.00000000}, 00060 { 2, 470.66183472, 384.00000000, 463.77709961, 384.00000000}, 00061 { 36, 941.68115234, 384.00000000, 925.83496094, 384.00000000}, 00062 { 22, 712.12817383, 384.00000000, 702.74102783, 384.00000000}, 00063 { 46, 660.98260498, 384.00000000, 653.19744873, 384.00000000}, 00064 { 47, 412.31518555, 384.00000000, 405.41717529, 384.00000000}, 00065 { 7, 476.52420044, 384.00000000, 470.44110107, 384.00000000}, 00066 { 17, 592.48101807, 384.00000000, 586.03741455, 384.00000000}, 00067 { 42, 538.14855957, 384.00000000, 532.17541504, 384.00000000}, 00068 { 27, 705.59130859, 384.00000000, 697.47851563, 384.00000000}, 00069 { 31, 374.95474243, 384.00000000, 367.96514893, 384.00000000}, 00070 }; 00071 00072 // Observations for KF#1. GT pose: 10 -> 0.502744 -0.099901 0.000000 0.526224 -0.526224 0.472322 -0.472322 00073 basic_stereo_dataset_entry_t dataset1[] = { 00074 { 39, 538.84362793, 384.00000000, 511.19470215, 384.00000000}, 00075 { 35, 492.97552490, 384.00000000, 467.92294312, 384.00000000}, 00076 { 28, 545.65521240, 384.00000000, 525.04907227, 384.00000000}, 00077 { 37, 615.96533203, 384.00000000, 597.56689453, 384.00000000}, 00078 { 6, 608.14080811, 384.00000000, 593.25360107, 384.00000000}, 00079 { 13, 443.73529053, 384.00000000, 430.62341309, 384.00000000}, 00080 { 24, 367.12982178, 384.00000000, 353.96624756, 384.00000000}, 00081 { 3, 570.37628174, 384.00000000, 559.49926758, 384.00000000}, 00082 { 26, 546.97644043, 384.00000000, 536.41058350, 384.00000000}, 00083 { 32, 559.28576660, 384.00000000, 549.18865967, 384.00000000}, 00084 { 8, 693.45947266, 384.00000000, 680.74969482, 384.00000000}, 00085 { 1, 711.24810791, 384.00000000, 698.11474609, 384.00000000}, 00086 { 12, 423.10546875, 384.00000000, 413.32467651, 384.00000000}, 00087 { 5, 533.79895020, 384.00000000, 525.03625488, 384.00000000}, 00088 { 2, 424.05053711, 384.00000000, 414.72103882, 384.00000000}, 00089 { 11, 736.43890381, 384.00000000, 723.78930664, 384.00000000}, 00090 { 22, 715.67138672, 384.00000000, 704.84838867, 384.00000000}, 00091 { 47, 334.04904175, 384.00000000, 324.01797485, 384.00000000}, 00092 { 7, 433.45681763, 384.00000000, 425.53250122, 384.00000000}, 00093 { 46, 653.96527100, 384.00000000, 645.04693604, 384.00000000}, 00094 { 42, 512.31976318, 384.00000000, 505.06820679, 384.00000000}, 00095 { 17, 575.66925049, 384.00000000, 568.13891602, 384.00000000}, 00096 { 36, 988.72906494, 384.00000000, 970.40332031, 384.00000000}, 00097 { 9, 558.54290771, 384.00000000, 551.54156494, 384.00000000}, 00098 { 31, 268.40109253, 384.00000000, 257.69583130, 384.00000000}, 00099 { 0, 448.58941650, 384.00000000, 441.61938477, 384.00000000}, 00100 { 40, 608.29187012, 384.00000000, 600.95947266, 384.00000000}, 00101 { 27, 698.85803223, 384.00000000, 689.85357666, 384.00000000}, 00102 { 16, 550.01416016, 384.00000000, 543.33697510, 384.00000000}, 00103 { 15, 486.11828613, 384.00000000, 479.63989258, 384.00000000}, 00104 { 10, 479.26995850, 384.00000000, 472.76791382, 384.00000000}, 00105 { 48, 358.30957031, 384.00000000, 350.39227295, 384.00000000}, 00106 { 38, 390.95483398, 384.00000000, 383.80099487, 384.00000000}, 00107 { 29, 392.46899414, 384.00000000, 385.34008789, 384.00000000}, 00108 }; 00109 00110 int main(int argc, char**argv) 00111 { 00112 my_srba_t rba; // Create an empty RBA problem 00113 00114 // -------------------------------------------------------------------------------- 00115 // Set parameters 00116 // -------------------------------------------------------------------------------- 00117 rba.setVerbosityLevel( 1 ); // 0: None; 1:Important only; 2:Verbose 00118 00119 rba.parameters.srba.use_robust_kernel = true; 00120 rba.parameters.obs_noise.std_noise_observations = 0.5; // pixels 00121 00122 // =========== Topology parameters =========== 00123 rba.parameters.srba.max_tree_depth = 3; 00124 rba.parameters.srba.max_optimize_depth = 3; 00125 // =========================================== 00126 00127 // Set camera calib: 00128 mrpt::utils::TCamera & lc = rba.parameters.sensor.camera_calib.leftCamera; 00129 lc.ncols = 1024; 00130 lc.nrows = 768; 00131 lc.cx(512); 00132 lc.cy(384); 00133 lc.fx(200); 00134 lc.fy(150); 00135 lc.dist.setZero(); 00136 rba.parameters.sensor.camera_calib.rightCamera = lc; 00137 rba.parameters.sensor.camera_calib.rightCameraPose.fromString("[0.2 0 0 1 0 0 0]"); // [X Y Z qr qx qy qz] 00138 00139 // Sensor pose on the robot parameters: 00140 rba.parameters.sensor_pose.relative_pose = mrpt::poses::CPose3D(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90) ); // Set camera pointing forwards (camera's +Z is robot +X) 00141 00142 // Alternatively, parameters can be loaded from an .ini-like config file 00143 // ----------------------------------------------------------------------- 00144 // rba.parameters.loadFromConfigFileName("config_file.cfg", "srba"); 00145 //rba.sensor_params.camera_calib.loadFromConfigFile("CAMERA","config_file.cfg"); 00146 00147 // -------------------------------------------------------------------------------- 00148 // Dump parameters to console (for checking/debugging only) 00149 // -------------------------------------------------------------------------------- 00150 //cout << "RBA parameters:\n-----------------\n"; 00151 //rba.parameters.dumpToConsole(); 00152 00153 00154 // -------------------------------------------------------------------------------- 00155 // Define observations of KF #0: 00156 // -------------------------------------------------------------------------------- 00157 my_srba_t::new_kf_observations_t list_obs; 00158 my_srba_t::new_kf_observation_t obs_field; 00159 00160 obs_field.is_fixed = false; // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated) 00161 #if 1 00162 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) 00163 #else 00164 obs_field.is_unknown_with_init_val = true; // Set default relative position of unknown new landmarks... 00165 obs_field.setRelPos( mrpt::math::TPoint3D(0.,0.,1.) ); // ...to (x,y,z)=(0,0,1), in front of the camera. 00166 #endif 00167 00168 for (size_t i=0;i<sizeof(dataset0)/sizeof(dataset0[0]);i++) 00169 { 00170 obs_field.obs.feat_id = dataset0[i].landmark_id; 00171 obs_field.obs.obs_data.left_px.x = dataset0[i].l_px_x; 00172 obs_field.obs.obs_data.left_px.y = dataset0[i].l_px_y; 00173 obs_field.obs.obs_data.right_px.x = dataset0[i].r_px_x; 00174 obs_field.obs.obs_data.right_px.y = dataset0[i].r_px_y; 00175 list_obs.push_back( obs_field ); 00176 } 00177 00178 00179 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00180 // ============================================================================================ 00181 my_srba_t::TNewKeyFrameInfo new_kf_info; 00182 rba.define_new_keyframe( 00183 list_obs, // Input observations for the new KF 00184 new_kf_info, // Output info 00185 true // Also run local optimization? 00186 ); 00187 00188 cout << "Created KF #" << new_kf_info.kf_id 00189 << " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl 00190 << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl 00191 << "-------------------------------------------------------" << endl; 00192 00193 00194 // -------------------------------------------------------------------------------- 00195 // Define observations of KF #1: 00196 // -------------------------------------------------------------------------------- 00197 list_obs.clear(); 00198 00199 for (size_t i=0;i<sizeof(dataset1)/sizeof(dataset1[0]);i++) 00200 { 00201 obs_field.obs.feat_id = dataset1[i].landmark_id; 00202 obs_field.obs.obs_data.left_px.x = dataset1[i].l_px_x; 00203 obs_field.obs.obs_data.left_px.y = dataset1[i].l_px_y; 00204 obs_field.obs.obs_data.right_px.x = dataset1[i].r_px_x; 00205 obs_field.obs.obs_data.right_px.y = dataset1[i].r_px_y; 00206 list_obs.push_back( obs_field ); 00207 } 00208 00209 // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. 00210 // ============================================================================================ 00211 rba.define_new_keyframe( 00212 list_obs, // Input observations for the new KF 00213 new_kf_info, // Output info 00214 true // Also run local optimization? 00215 ); 00216 00217 cout << "Created KF #" << new_kf_info.kf_id 00218 << " | # kf-to-kf edges created:" << new_kf_info.created_edge_ids.size() << endl 00219 << "Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl 00220 << "-------------------------------------------------------" << endl; 00221 00222 00223 // Dump the relative pose of KF#0 wrt KF#1: 00224 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; 00225 00226 // -------------------------------------------------------------------------------- 00227 // Saving RBA graph as a DOT file: 00228 // -------------------------------------------------------------------------------- 00229 const string sFil = "graph.dot"; 00230 cout << "Saving final graph of KFs and LMs to: " << sFil << endl; 00231 rba.save_graph_as_dot(sFil, true /* LMs=save */); 00232 cout << "Done.\n"; 00233 00234 // -------------------------------------------------------------------------------- 00235 // Show 3D view of the resulting map: 00236 // -------------------------------------------------------------------------------- 00237 my_srba_t::TOpenGLRepresentationOptions opengl_options; 00238 mrpt::opengl::CSetOfObjectsPtr rba_3d = mrpt::opengl::CSetOfObjects::Create(); 00239 00240 rba.build_opengl_representation( 00241 0, // Root KF, 00242 opengl_options, // Rendering options 00243 rba_3d // Output scene 00244 ); 00245 00246 // Display: 00247 #if MRPT_HAS_WXWIDGETS 00248 mrpt::gui::CDisplayWindow3D win("RBA results",640,480); 00249 { 00250 mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); 00251 scene->insert(rba_3d); 00252 win.unlockAccess3DScene(); 00253 } 00254 win.setCameraZoom( 4 ); 00255 win.repaint(); 00256 00257 cout << "Press any key or close window to exit.\n"; 00258 win.waitForKey(); 00259 #endif 00260 00261 return 0; // All ok 00262 }