void run_test(const mrpt::poses::CPose3D &incr) { my_srba_t rba; // Create an empty RBA problem rba.get_time_profiler().disable(); // -------------------------------------------------------------------------------- // Set parameters // -------------------------------------------------------------------------------- rba.setVerbosityLevel( 0 ); // 0: None; 1:Important only; 2:Verbose rba.parameters.srba.use_robust_kernel = false; // =========== Topology parameters =========== rba.parameters.srba.max_tree_depth = 3; rba.parameters.srba.max_optimize_depth = 3; // =========================================== // -------------------------------------------------------------------------------- // Define observations of KF #0: // -------------------------------------------------------------------------------- my_srba_t::new_kf_observations_t list_obs; my_srba_t::new_kf_observation_t obs_field; obs_field.is_fixed = false; // Landmarks have unknown relative positions (i.e. treat them as unknowns to be estimated) 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) for (size_t i=0;i<sizeof(dummy_obs)/sizeof(dummy_obs[0]);i++) { obs_field.obs.feat_id = dummy_obs[i].landmark_id; obs_field.obs.obs_data.pt.x = dummy_obs[i].x; obs_field.obs.obs_data.pt.y = dummy_obs[i].y; obs_field.obs.obs_data.pt.z = dummy_obs[i].z; list_obs.push_back( obs_field ); } // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. // ============================================================================================ my_srba_t::TNewKeyFrameInfo new_kf_info; rba.define_new_keyframe( list_obs, // Input observations for the new KF new_kf_info, // Output info true // Also run local optimization? ); // -------------------------------------------------------------------------------- // Define observations of next KF: // -------------------------------------------------------------------------------- list_obs.clear(); for (size_t i=0;i<sizeof(dummy_obs)/sizeof(dummy_obs[0]);i++) { obs_field.obs.feat_id = dummy_obs[i].landmark_id; obs_field.obs.obs_data.pt.x = dummy_obs[i].x; obs_field.obs.obs_data.pt.y = dummy_obs[i].y; obs_field.obs.obs_data.pt.z = dummy_obs[i].z; if (INVERSE_INCR) incr.inverseComposePoint( obs_field.obs.obs_data.pt.x, obs_field.obs.obs_data.pt.y, obs_field.obs.obs_data.pt.z, obs_field.obs.obs_data.pt.x, obs_field.obs.obs_data.pt.y, obs_field.obs.obs_data.pt.z); else incr.composePoint(obs_field.obs.obs_data.pt, obs_field.obs.obs_data.pt); list_obs.push_back( obs_field ); } // Here happens the main stuff: create Key-frames, build structures, run optimization, etc. // ============================================================================================ rba.define_new_keyframe( list_obs, // Input observations for the new KF new_kf_info, // Output info true // Also run local optimization? ); mrpt::poses::CPose3D estIncr = rba.get_k2k_edges()[0].inv_pose; if (INVERSE_INCR) estIncr.inverse(); EXPECT_NEAR( (incr.getHomogeneousMatrixVal()-estIncr.getHomogeneousMatrixVal()).array().abs().sum(),0, 1e-3) << "=> Ground truth: " << incr << " Inverse: " << (INVERSE_INCR ? "YES":"NO") << endl << "=> inv_pose of KF-to-KF edge #0 (relative pose of KF#0 wrt KF#1):\n" << estIncr << endl << "=> Optimization error: " << new_kf_info.optimize_results.total_sqr_error_init << " -> " << new_kf_info.optimize_results.total_sqr_error_final << endl; }