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