// Set the given cloud as the target to which the templates will be aligned
 void
 setTargetCloud (FeatureCloud &target_cloud)
 {
     target_ = target_cloud;
     sac_ia_.setInputTarget (target_cloud.getPointCloud ());
     sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
 }
void TemplateAlignment::align( FeatureCloud &template_cloud, TemplateAlignment::Result &result )
{
	sac_ia_.setInputCloud( template_cloud.getPointCloud() );
	sac_ia_.setSourceFeatures( template_cloud.getLocalFeatures() );
	
	pcl::PointCloud<pcl::PointXYZ> registration_output;
	sac_ia_.align( registration_output );
	
	result.fitness_score = (float)sac_ia_.getFitnessScore( max_correspondence_distance_ );
	result.final_transformation = sac_ia_.getFinalTransformation();
}
示例#3
0
int sac_ia_alignment(pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {

	pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>(*prev_cloud));

	std::cout << "Performing Sample Consensus Initial Alignment.. "
			<< std::endl;
	FeatureCloud targetCloud;
	FeatureCloud templateCloud;
	targetCloud.setInputCloud(temp_cloud);
	templateCloud.setInputCloud(cloud_in);


	TemplateAlignment templateAlign;
	templateAlign.addTemplateCloud(templateCloud);
	templateAlign.setTargetCloud(targetCloud);

	Result bestAlignment;
	std::cout << "entering alignment" << std::endl;
	templateAlign.findBestAlignment(bestAlignment);
	std::cout << "exiting alignment" << std::endl;

	printf("Fitness Score: %f \n", bestAlignment.fitness_score);

	Eigen::Matrix3f rotation = bestAlignment.final_transformation.block<3,3>(0, 0);
	Eigen::Vector3f translation =
			bestAlignment.final_transformation.block<3,1>(0, 3);

	printf("\n");
	printf("    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1),
			rotation(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1),
			rotation(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1),
			rotation(2, 2));
	printf("\n");
	printf("t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1),
			translation(2));

	pcl::transformPointCloud(*templateCloud.getPointCloud(), *cloud_in,
			bestAlignment.final_transformation);

	std::cout << "***Initial alignment complete***" << std::endl;

}
void TemplateAlignment::setTargetCloud( FeatureCloud &target_cloud )
{
	target_ = target_cloud;
	sac_ia_.setInputTarget( target_cloud.getPointCloud() );
	sac_ia_.setTargetFeatures( target_cloud.getLocalFeatures() );
}