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;
}
