Function testRigidTransformFitting

examples/cpp_example.cpp:853–968

An example function showing how to fit calculate a rigid transformation from a set of 3D-3D correspondences by Graph-Cut RANSAC

// An example function showing how to fit calculate a rigid transformation from a set of 3D-3D correspondences by Graph-Cut RANSAC
void testRigidTransformFitting(
	const std::string& ground_truth_pose_path_, // The path where the ground truth pose can be found
	const std::string& points_path_, // The path of the points 
	const std::string& inliers_point_path_, // The path where the inlier correspondences are saved
	const double confidence_, // The RANSAC confidence value
	const double inlier_outlier_threshold_, // The used inlier-outlier threshold in GC-RANSAC.
	const double spatial_coherence_weight_, // The weight of the spatial coherence term in the graph-cut energy minimization.
	const double sphere_radius_,  // The radius of the sphere used for determining the neighborhood-graph
	const double minimum_inlier_ratio_for_sprt_) // An assumption about the minimum inlier ratio used for the SPRT test
{
	// The image and world points stored as rows in the matrix. Each row is of format 'u v x y z'.
	cv::Mat points;

	// Loading the 3D-3D correspondences 
	gcransac::utils::loadPointsFromFile<6, 1, false>(points, // The points in the image
		points_path_.c_str()); // The path where the image points are stored

	// Load the ground truth pose
	Eigen::Matrix<double, 4, 4> ground_truth_T;
	if (!utils::loadMatrix<double, 4, 4>(ground_truth_pose_path_,
		ground_truth_T))
	{
		printf("An error occured when loading the reference camera pose from '%s'\n",
			ground_truth_pose_path_.c_str());
		return;
	}

	// Initialize the neighborhood used in Graph-cut RANSAC and, perhaps,
	// in the sampler if NAPSAC or Progressive-NAPSAC sampling is applied.
	std::chrono::time_point<std::chrono::system_clock> start, end; // Variables for time measurement
	start = std::chrono::system_clock::now(); // The starting time of the neighborhood calculation
	// TODO: generalize the grid class to handle not only point correspondences
	neighborhood::FlannNeighborhoodGraph neighborhood(&points, // The data points
		sphere_radius_); // The radius of the neighborhood sphere used for determining the neighborhood structure
	end = std::chrono::system_clock::now(); // The end time of the neighborhood calculation
	std::chrono::duration<double> elapsed_seconds = end - start; // The elapsed time in seconds
	printf("Neighborhood calculation time = %f secs\n", elapsed_seconds.count());

	// Apply Graph-cut RANSAC
	utils::DefaultRigidTransformationEstimator estimator; // The estimator used for the pose fitting
	RigidTransformation model; // The estimated model parameters

	// Initialize the samplers
	// The main sampler is used inside the local optimization
	sampler::UniformSampler main_sampler(&points); // The data points
	sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization

	// Checking if the samplers are initialized successfully.
	if (!main_sampler.isInitialized() ||
		!local_optimization_sampler.isInitialized())
	{
		fprintf(stderr, "One of the samplers is not initialized successfully.\n");
		return;
	}

	// Initializing SPRT test
	preemption::EmptyPreemptiveVerfication<utils::DefaultRigidTransformationEstimator> preemptive_verification;