欢迎点云相关产学研的学者和团体加入我们。
接下来我们定义一个设置目标点云的方法,该目标点云正是模板点云需要对其的数据集,即设置SAC-IA算法的输入点云数据。
// 设置模板点云需要对其的目标点云数据
void
setTargetCloud (FeatureCloud &target_cloud)
{
target_ =target_cloud;
sac_ia_.setInputTarget (target_cloud.getPointCloud ());
sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
}
然后,我们定义一个方法来指定哪些模板点云将要被配准,通过该方法将模板点云FeatureClouds的对象存储到TemplateAlignment对象的模板序列中。
// 添加给定的点云到需要配准的模板序列中
void
addTemplateCloud (FeatureCloud &template_cloud)
{
templates_.push_back (template_cloud);
}
接着我们定义配准方法,这个方法以模板点云为输入,并且将它跟已经通过函数setInputTarget()指定的目标云进行匹配.,align()方法内部调用SAC-IA算法实现我们的的输入点云与目标点云的配准,虽然内部配准后可以得到变换后的模板点云数据,但这里我们忽略该数据,相反,此处需要SAC-IC方法得到的配准拟合系数和最终的变换矩阵(从源点云到目标点云的刚体变换矩阵),并且我们将它们存储到Result结构体中输出给调用函数)。
// 配准模板点云与setTargetCloud ()设定的目标点云
void
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 ();
}
因为这个类被设计为可以和多个模板进行配准,这里也定义一个方法即将所有的模板与目标云进行配准并且将结果保存成Result的一个向量返回。
// 配准所有的模板点云与setTargetCloud ()设定的目标点云
void
alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
{
results.resize (templates_.size ());
for (size_t i=0; i<templates_.size (); ++i)
{
align (templates_[i], results[i]);
}
}
最后,我们定义一个方法将所有的模板点云与目标点云进行配准并且返回其最佳的拟合模板点云索引与相应的拟合系数。
// 配准所有模板与目标点云获取最拟合的模板
int
findBestAlignment (TemplateAlignment::Result &result)
{
std::vector<Result, Eigen::aligned_allocator<Result> > results;
alignAll (results);
// 获取最佳匹配的模板,即具有最小拟合系数的点云
float lowest_score=std::numeric_limits<float>::infinity ();
int best_template=0;
for (size_t i=0; i<results.size (); ++i)
{
const Result &r = results[i];
if (r.fitness_score<lowest_score)
{
lowest_score=r.fitness_score;
best_template= (int) i;
}
}
// 输出最佳匹配模板
result= results[best_template];
return (best_template);
}
现在有了一个处理配准模板与目标的类,我们把它应用到脸部配准问题上,在提供的数据文件中,已经包含了从不同角度观察人脸所创建的六个不同视角的点云模板,每个模板数据是经过下采样处理并且手工将非脸部位裁剪掉后得到的,在下列代码,我们演示如何用我们的TemplateAlignment类来查找在目标点云中人脸的位姿。
未完待续,敬请关注“对齐对象模板到点云(4)”的其他内容。