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