<commit_before>/**\file cloud_matcher.hpp
 * \brief Description...
 *
 * @version 1.0
 * @author <NAME>
 */

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <includes>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
#include <dynamic_robot_localization/cloud_matchers/cloud_matcher.h>
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </includes>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

namespace dynamic_robot_localization {

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <imports>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </imports>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// =============================================================================  <public-section>  ============================================================================
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <constructors-destructor>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
template<typename PointT>
CloudMatcher<PointT>::CloudMatcher() :
		match_only_keypoints_(false),
		display_cloud_aligment_(false),
		maximum_number_of_displayed_correspondences_(0) {}
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </constructors-destructor>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <CloudMatcher-functions>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
template<typename PointT>
void CloudMatcher<PointT>::setupConfigurationFromParameterServer(ros::NodeHandlePtr& node_handle, ros::NodeHandlePtr& private_node_handle, std::string configuration_namespace) {
	std::string final_param_name;
	std::string search_namespace = private_node_handle->getNamespace() + "/" + configuration_namespace;
	if (ros::param::search(search_namespace, "match_only_keypoints", final_param_name)) { private_node_handle->param(final_param_name, match_only_keypoints_, false); }
	if (ros::param::search(search_namespace, "display_cloud_aligment", final_param_name)) { private_node_handle->param(final_param_name, display_cloud_aligment_, false); }
	if (ros::param::search(search_namespace, "maximum_number_of_displayed_correspondences", final_param_name)) { private_node_handle->param(final_param_name, maximum_number_of_displayed_correspondences_, 0); } // show all

	cloud_publisher_ = typename CloudPublisher<PointT>::Ptr(new CloudPublisher<PointT>());
	cloud_publisher_->setParameterServerArgumentToLoadTopicName(configuration_namespace + "registered_cloud_publish_topic");
	cloud_publisher_->setupConfigurationFromParameterServer(node_handle, private_node_handle, configuration_namespace);

	// subclass must set cloud_matcher_ ptr
	if (cloud_matcher_) {
		double max_correspondence_distance;
		double transformation_epsilon;
		double euclidean_fitness_epsilon;
		int max_number_of_registration_iterations;
		int max_number_of_ransac_iterations;
		double ransac_outlier_rejection_threshold;

		if (ros::param::search(search_namespace, "max_correspondence_distance", final_param_name)) { private_node_handle->param(final_param_name, max_correspondence_distance, 0.1); }
		if (ros::param::search(search_namespace, "transformation_epsilon", final_param_name)) { private_node_handle->param(final_param_name, transformation_epsilon, 1e-8); }
		if (ros::param::search(search_namespace, "euclidean_fitness_epsilon", final_param_name)) { private_node_handle->param(final_param_name, euclidean_fitness_epsilon, 1e-6); }
		if (ros::param::search(search_namespace, "max_number_of_registration_iterations", final_param_name)) { private_node_handle->param(final_param_name, max_number_of_registration_iterations, 250); }
		if (ros::param::search(search_namespace, "max_number_of_ransac_iterations", final_param_name)) { private_node_handle->param(final_param_name, max_number_of_ransac_iterations, 250); }
		if (ros::param::search(search_namespace, "ransac_outlier_rejection_threshold", final_param_name)) { private_node_handle->param(final_param_name, ransac_outlier_rejection_threshold, 0.05); }

		cloud_matcher_->setMaxCorrespondenceDistance(max_correspondence_distance);
		cloud_matcher_->setTransformationEpsilon(transformation_epsilon);
		cloud_matcher_->setEuclideanFitnessEpsilon(euclidean_fitness_epsilon);
		cloud_matcher_->setMaximumIterations(max_number_of_registration_iterations);
		cloud_matcher_->setRANSACIterations(max_number_of_ransac_iterations);
		cloud_matcher_->setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold);

		if (max_number_of_ransac_iterations > 0) {
			typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::Ptr rej_sac(new pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>());
			rej_sac->setMaximumIterations(max_number_of_ransac_iterations);
			rej_sac->setInlierThreshold(ransac_outlier_rejection_threshold);
			cloud_matcher_->addCorrespondenceRejector(rej_sac);
		}


		correspondence_estimation_ptr_.reset();
		int correspondence_estimation_k;
		if (ros::param::search(search_namespace, "correspondence_estimation_k", final_param_name)) { private_node_handle->param(final_param_name, correspondence_estimation_k, 10); }

		std::string correspondence_estimation_method;
		if (ros::param::search(search_namespace, "correspondence_estimation_approach", final_param_name)) { private_node_handle->param(final_param_name, correspondence_estimation_method, std::string("")); }

		if (correspondence_estimation_method == "CorrespondenceEstimation") {
			correpondence_estimation_approach_ = CorrespondenceEstimation;
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(new CorrespondenceEstimationTimed<PointT, PointT, float>());
		} else if(correspondence_estimation_method == "CorrespondenceEstimationLookupTable") {
			correpondence_estimation_approach_ = CorrespondenceEstimationLookupTable;
			CorrespondenceEstimationLookupTableTimed<PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationLookupTableTimed<PointT, PointT, float>();
			double map_cell_resolution = 0.01, map_margin_x = 1.0, map_margin_y = 1.0, map_margin_z = 1.0;
			double sensor_cell_resolution = 0.01, sensor_margin_x = 1.0, sensor_margin_y = 1.0, sensor_margin_z = 1.0;
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_cell_resolution", final_param_name)) { private_node_handle->param(final_param_name, map_cell_resolution, 0.01); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_x", final_param_name)) { private_node_handle->param(final_param_name, map_margin_x, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_y", final_param_name)) { private_node_handle->param(final_param_name, map_margin_y, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_z", final_param_name)) { private_node_handle->param(final_param_name, map_margin_z, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_cell_resolution", final_param_name)) { private_node_handle->param(final_param_name, sensor_cell_resolution, 0.01); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_x", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_x, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_y", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_y, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_z", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_z, 1.0); }
			correspondence_estimation_raw_ptr_->getTargetCorrespondencesLookupTable().setCellResolution(map_cell_resolution);
			correspondence_estimation_raw_ptr_->getTargetCorrespondencesLookupTable().setLookupTableMargin(Eigen::Vector4f(map_margin_x, map_margin_y, map_margin_z, 0.0));
			correspondence_estimation_raw_ptr_->getSourceCorrespondencesLookupTable().setCellResolution(sensor_cell_resolution);
			correspondence_estimation_raw_ptr_->getSourceCorrespondencesLookupTable().setLookupTableMargin(Eigen::Vector4f(sensor_margin_x, sensor_margin_y, sensor_margin_z, 0.0));
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationBackProjection") {
			correpondence_estimation_approach_ = CorrespondenceEstimationBackProjection;
			CorrespondenceEstimationBackProjectionTimed<PointT, PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationBackProjectionTimed<PointT, PointT, PointT, float>();
			correspondence_estimation_raw_ptr_->setKSearch(correspondence_estimation_k);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationNormalShooting") {
			correpondence_estimation_approach_ = CorrespondenceEstimationNormalShooting;
			CorrespondenceEstimationNormalShootingTimed<PointT, PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationNormalShootingTimed<PointT, PointT, PointT, float>();
			correspondence_estimation_raw_ptr_->setKSearch(correspondence_estimation_k);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationOrganizedProjection") {
			correpondence_estimation_approach_ = CorrespondenceEstimationOrganizedProjection;
			CorrespondenceEstimationOrganizedProjectionTimed<PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationOrganizedProjectionTimed<PointT, PointT, float>();
			double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5;
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/fx", final_param_name)) { private_node_handle->param(final_param_name, fx, 525.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/fy", final_param_name)) { private_node_handle->param(final_param_name, fy, 525.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/cx", final_param_name)) { private_node_handle->param(final_param_name, cx, 319.5); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/cy", final_param_name)) { private_node_handle->param(final_param_name, cy, 239.5); }
			correspondence_estimation_raw_ptr_->setFocalLengths(fx, fy);
			correspondence_estimation_raw_ptr_->setCameraCenters(cx, cy);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		}

		if (correspondence_estimation_ptr_) {
			cloud_matcher_->setCorrespondenceEstimation(correspondence_estimation_ptr_);
		}


		std::string transformation_estimation_method;
		if (ros::param::search(search_namespace, "transformation_estimation_approach", final_param_name)) { private_node_handle->param(final_param_name, transformation_estimation_method, std::string("")); }
		if (transformation_estimation_method == "TransformationEstimation2D") {
			transformation_estimation_approach_ = TransformationEstimation2D;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimation2DTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationDualQuaternion") {
			transformation_estimation_approach_ = TransformationEstimationDualQuaternion;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationDualQuaternionTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationLM") {
			transformation_estimation_approach_ = TransformationEstimationLM;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationLMTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlane") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlane;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneLLS") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneLLS;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneLLSTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneLLSWeighted") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneLLSWeighted;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneLLSWeightedTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneWeighted") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneWeighted;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneWeightedTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationSVD") {
			transformation_estimation_approach_ = TransformationEstimationSVD;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationSVDTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationSVDScale") {
			transformation_estimation_approach_ = TransformationEstimationSVDScale;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationSVDScaleTimed<PointT, PointT, float>());
		}

		if (transformation_estimation_ptr_) {
			cloud_matcher_->setTransformationEstimation(transformation_estimation_ptr_);
		}


		setupRegistrationVisualizer();
	}
}


template<typename PointT>
void CloudMatcher<PointT>::setupReferenceCloud(typename pcl::PointCloud<PointT>::Ptr& reference_cloud, typename pcl::PointCloud<PointT>::Ptr& reference_cloud_keypoints,
		typename pcl::search::KdTree<PointT>::Ptr& search_method) {
	// subclass must set cloud_matcher_ ptr
	if (cloud_matcher_) {
		cloud_matcher_->setInputTarget(reference_cloud);
		cloud_matcher_->setSearchMethodTarget(search_method, true);
		cloud_matcher_->getCorrespondenceEstimation()->setSearchMethodTarget(search_method, false);
	}

	if (registration_visualizer_) {
		registration_visualizer_->setTargetCloud(*reference_cloud);
	}
}


template<typename PointT>
bool CloudMatcher<PointT>::registerCloud(typename pcl::PointCloud<PointT>::Ptr& ambient_pointcloud,
		typename pcl::search::KdTree<PointT>::Ptr& ambient_pointcloud_search_method,<commit_msg>Improved ndt compatibility.<commit_after>/**\file cloud_matcher.hpp
 * \brief Description...
 *
 * @version 1.0
 * @author <NAME>
 */

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <includes>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
#include <dynamic_robot_localization/cloud_matchers/cloud_matcher.h>
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </includes>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

namespace dynamic_robot_localization {

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <imports>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </imports>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// =============================================================================  <public-section>  ============================================================================
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <constructors-destructor>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
template<typename PointT>
CloudMatcher<PointT>::CloudMatcher() :
		match_only_keypoints_(false),
		display_cloud_aligment_(false),
		maximum_number_of_displayed_correspondences_(0) {}
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </constructors-destructor>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <CloudMatcher-functions>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
template<typename PointT>
void CloudMatcher<PointT>::setupConfigurationFromParameterServer(ros::NodeHandlePtr& node_handle, ros::NodeHandlePtr& private_node_handle, std::string configuration_namespace) {
	std::string final_param_name;
	std::string search_namespace = private_node_handle->getNamespace() + "/" + configuration_namespace;
	if (ros::param::search(search_namespace, "match_only_keypoints", final_param_name)) { private_node_handle->param(final_param_name, match_only_keypoints_, false); }
	if (ros::param::search(search_namespace, "display_cloud_aligment", final_param_name)) { private_node_handle->param(final_param_name, display_cloud_aligment_, false); }
	if (ros::param::search(search_namespace, "maximum_number_of_displayed_correspondences", final_param_name)) { private_node_handle->param(final_param_name, maximum_number_of_displayed_correspondences_, 0); } // show all

	cloud_publisher_ = typename CloudPublisher<PointT>::Ptr(new CloudPublisher<PointT>());
	cloud_publisher_->setParameterServerArgumentToLoadTopicName(configuration_namespace + "registered_cloud_publish_topic");
	cloud_publisher_->setupConfigurationFromParameterServer(node_handle, private_node_handle, configuration_namespace);

	// subclass must set cloud_matcher_ ptr
	if (cloud_matcher_) {
		double max_correspondence_distance;
		double transformation_epsilon;
		double euclidean_fitness_epsilon;
		int max_number_of_registration_iterations;
		int max_number_of_ransac_iterations;
		double ransac_outlier_rejection_threshold;

		if (ros::param::search(search_namespace, "max_correspondence_distance", final_param_name)) { private_node_handle->param(final_param_name, max_correspondence_distance, 0.1); }
		if (ros::param::search(search_namespace, "transformation_epsilon", final_param_name)) { private_node_handle->param(final_param_name, transformation_epsilon, 1e-8); }
		if (ros::param::search(search_namespace, "euclidean_fitness_epsilon", final_param_name)) { private_node_handle->param(final_param_name, euclidean_fitness_epsilon, 1e-6); }
		if (ros::param::search(search_namespace, "max_number_of_registration_iterations", final_param_name)) { private_node_handle->param(final_param_name, max_number_of_registration_iterations, 250); }
		if (ros::param::search(search_namespace, "max_number_of_ransac_iterations", final_param_name)) { private_node_handle->param(final_param_name, max_number_of_ransac_iterations, 250); }
		if (ros::param::search(search_namespace, "ransac_outlier_rejection_threshold", final_param_name)) { private_node_handle->param(final_param_name, ransac_outlier_rejection_threshold, 0.05); }

		cloud_matcher_->setMaxCorrespondenceDistance(max_correspondence_distance);
		cloud_matcher_->setTransformationEpsilon(transformation_epsilon);
		cloud_matcher_->setEuclideanFitnessEpsilon(euclidean_fitness_epsilon);
		cloud_matcher_->setMaximumIterations(max_number_of_registration_iterations);
		cloud_matcher_->setRANSACIterations(max_number_of_ransac_iterations);
		cloud_matcher_->setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold);

		if (max_number_of_ransac_iterations > 0) {
			typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::Ptr rej_sac(new pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>());
			rej_sac->setMaximumIterations(max_number_of_ransac_iterations);
			rej_sac->setInlierThreshold(ransac_outlier_rejection_threshold);
			cloud_matcher_->addCorrespondenceRejector(rej_sac);
		}


		correspondence_estimation_ptr_.reset();
		int correspondence_estimation_k;
		if (ros::param::search(search_namespace, "correspondence_estimation_k", final_param_name)) { private_node_handle->param(final_param_name, correspondence_estimation_k, 10); }

		std::string correspondence_estimation_method;
		if (ros::param::search(search_namespace, "correspondence_estimation_approach", final_param_name)) { private_node_handle->param(final_param_name, correspondence_estimation_method, std::string("")); }

		if (correspondence_estimation_method == "CorrespondenceEstimation") {
			correpondence_estimation_approach_ = CorrespondenceEstimation;
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(new CorrespondenceEstimationTimed<PointT, PointT, float>());
		} else if(correspondence_estimation_method == "CorrespondenceEstimationLookupTable") {
			correpondence_estimation_approach_ = CorrespondenceEstimationLookupTable;
			CorrespondenceEstimationLookupTableTimed<PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationLookupTableTimed<PointT, PointT, float>();
			double map_cell_resolution = 0.01, map_margin_x = 1.0, map_margin_y = 1.0, map_margin_z = 1.0;
			double sensor_cell_resolution = 0.01, sensor_margin_x = 1.0, sensor_margin_y = 1.0, sensor_margin_z = 1.0;
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_cell_resolution", final_param_name)) { private_node_handle->param(final_param_name, map_cell_resolution, 0.01); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_x", final_param_name)) { private_node_handle->param(final_param_name, map_margin_x, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_y", final_param_name)) { private_node_handle->param(final_param_name, map_margin_y, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/map_margin_z", final_param_name)) { private_node_handle->param(final_param_name, map_margin_z, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_cell_resolution", final_param_name)) { private_node_handle->param(final_param_name, sensor_cell_resolution, 0.01); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_x", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_x, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_y", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_y, 1.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_lookup_table/sensor_margin_z", final_param_name)) { private_node_handle->param(final_param_name, sensor_margin_z, 1.0); }
			correspondence_estimation_raw_ptr_->getTargetCorrespondencesLookupTable().setCellResolution(map_cell_resolution);
			correspondence_estimation_raw_ptr_->getTargetCorrespondencesLookupTable().setLookupTableMargin(Eigen::Vector4f(map_margin_x, map_margin_y, map_margin_z, 0.0));
			correspondence_estimation_raw_ptr_->getSourceCorrespondencesLookupTable().setCellResolution(sensor_cell_resolution);
			correspondence_estimation_raw_ptr_->getSourceCorrespondencesLookupTable().setLookupTableMargin(Eigen::Vector4f(sensor_margin_x, sensor_margin_y, sensor_margin_z, 0.0));
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationBackProjection") {
			correpondence_estimation_approach_ = CorrespondenceEstimationBackProjection;
			CorrespondenceEstimationBackProjectionTimed<PointT, PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationBackProjectionTimed<PointT, PointT, PointT, float>();
			correspondence_estimation_raw_ptr_->setKSearch(correspondence_estimation_k);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationNormalShooting") {
			correpondence_estimation_approach_ = CorrespondenceEstimationNormalShooting;
			CorrespondenceEstimationNormalShootingTimed<PointT, PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationNormalShootingTimed<PointT, PointT, PointT, float>();
			correspondence_estimation_raw_ptr_->setKSearch(correspondence_estimation_k);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		} else if (correspondence_estimation_method == "CorrespondenceEstimationOrganizedProjection") {
			correpondence_estimation_approach_ = CorrespondenceEstimationOrganizedProjection;
			CorrespondenceEstimationOrganizedProjectionTimed<PointT, PointT, float>* correspondence_estimation_raw_ptr_ = new CorrespondenceEstimationOrganizedProjectionTimed<PointT, PointT, float>();
			double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5;
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/fx", final_param_name)) { private_node_handle->param(final_param_name, fx, 525.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/fy", final_param_name)) { private_node_handle->param(final_param_name, fy, 525.0); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/cx", final_param_name)) { private_node_handle->param(final_param_name, cx, 319.5); }
			if (ros::param::search(search_namespace, "correspondence_estimation_organized_projection/cy", final_param_name)) { private_node_handle->param(final_param_name, cy, 239.5); }
			correspondence_estimation_raw_ptr_->setFocalLengths(fx, fy);
			correspondence_estimation_raw_ptr_->setCameraCenters(cx, cy);
			correspondence_estimation_ptr_ = typename pcl::registration::CorrespondenceEstimationBase<PointT, PointT, float>::Ptr(correspondence_estimation_raw_ptr_);
		}

		if (correspondence_estimation_ptr_) {
			cloud_matcher_->setCorrespondenceEstimation(correspondence_estimation_ptr_);
		}


		std::string transformation_estimation_method;
		if (ros::param::search(search_namespace, "transformation_estimation_approach", final_param_name)) { private_node_handle->param(final_param_name, transformation_estimation_method, std::string("")); }
		if (transformation_estimation_method == "TransformationEstimation2D") {
			transformation_estimation_approach_ = TransformationEstimation2D;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimation2DTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationDualQuaternion") {
			transformation_estimation_approach_ = TransformationEstimationDualQuaternion;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationDualQuaternionTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationLM") {
			transformation_estimation_approach_ = TransformationEstimationLM;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationLMTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlane") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlane;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneLLS") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneLLS;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneLLSTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneLLSWeighted") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneLLSWeighted;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneLLSWeightedTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationPointToPlaneWeighted") {
			transformation_estimation_approach_ = TransformationEstimationPointToPlaneWeighted;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationPointToPlaneWeightedTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationSVD") {
			transformation_estimation_approach_ = TransformationEstimationSVD;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationSVDTimed<PointT, PointT, float>());
		} else if (transformation_estimation_method == "TransformationEstimationSVDScale") {
			transformation_estimation_approach_ = TransformationEstimationSVDScale;
			transformation_estimation_ptr_ = typename pcl::registration::TransformationEstimation<PointT, PointT, float>::Ptr(new TransformationEstimationSVDScaleTimed<PointT, PointT, float>());
		}

		if (transformation_estimation_ptr_) {
			cloud_matcher_->setTransformationEstimation(transformation_estimation_ptr_);
		}


		setupRegistrationVisualizer();
	}
}


template<typename PointT>
void CloudMatcher<PointT>::setupReferenceCloud(typename pcl::PointCloud<PointT>::Ptr& reference_cloud, typename pcl::PointCloud<PointT>::Ptr& reference_cloud_keypoints,
		typename pcl::search::KdTree<PointT>::Ptr& search_method) {
	// subclass must set cloud_matcher_ ptr
	if (cloud_matcher_) {
		cloud_matcher_->setInputTarget(reference_cloud);
		cloud_matcher_->setSearchMethodTarget(search_method, true);
		if (cloud_matcher_->getCorrespondenceEstimation())
			cloud_matcher_->getCorrespondenceEstimation()->setSearchMethodTarget(search_method, false);
	}

	if (registration_visualizer_) {
		registration_visualizer_->setTargetCloud(*reference_cloud);
	}
}


template<typename PointT>
bool CloudMatcher<PointT>::registerCloud(typename pcl::PointCloud<PointT>::Ptr& ambient_pointcloud,
		typename pcl::search::KdTree<PointT>::Ptr& ambient_pointcloud_search_method,
{'fraction_non_alphanumeric': 0.1649424225629657, 'fraction_numerical': 0.006096434509514133, 'mean_word_length': 11.776947285601889, 'pattern_counts': {'":': 0, '<': 1173, '<?xml version=': 0, '>': 1288, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3404525', 'n_tokens_mistral': 10710, 'n_tokens_neox': 9944, 'n_words': 1133}