void run_pc_filter_test(
const double map2_x_off, const double map2_tim_off,
const size_t expected_m1_count, const size_t expected_m2_count)
{
const double pts1[8][3] = {
{1, 0, 0}, {1.03, 0, 0}, {1, 1, 0}, {1.01, 1.02, 0},
{0, 1, 0}, {-0.01, 1.01, 0}, {-1, 0, 0}, {-1.01, 0.02, 0}};
const mrpt::poses::CPose3D pts1_pose(0, 0, 0, 0, 0, 0);
const mrpt::system::TTimeStamp pts1_tim = mrpt::system::now();
const mrpt::poses::CPose3D pts2_pose(0.5, 0, 0, 0, 0, 0);
const mrpt::system::TTimeStamp pts2_tim =
mrpt::system::timestampAdd(pts1_tim, 0.2 + map2_tim_off);
mrpt::maps::CSimplePointsMap map1, map2;
for (size_t i = 0; i < sizeof(pts1) / sizeof(pts1[0]); i++)
map1.insertPoint(pts1[i][0], pts1[i][1], pts1[i][2]);
for (size_t i = 0; i < sizeof(pts1) / sizeof(pts1[0]); i++)
{
double x, y, z;
pts2_pose.inverseComposePoint(
pts1[i][0], pts1[i][1], pts1[i][2], x, y, z);
// Introduce optionally, 1 outlier:
if (i == 1) x += map2_x_off;
map2.insertPoint(x, y, z);
}
mrpt::maps::CPointCloudFilterByDistance pc_filter;
mrpt::maps::CPointCloudFilterByDistance::TExtraFilterParams extra_params;
std::vector<bool> deletion_mask;
extra_params.out_deletion_mask = &deletion_mask;
pc_filter.filter(&map1, pts1_tim, pts1_pose, &extra_params);
EXPECT_EQ(map1.size(), expected_m1_count);
pc_filter.filter(&map2, pts2_tim, pts2_pose, &extra_params);
EXPECT_EQ(map2.size(), expected_m2_count);
}
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;
}