// 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();
}
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() );
}