PCL 3维点云的模板匹配
Doc 来自PCL官方文档 http://www.pointclouds.org/documentation/tutorials/template_alignment.php#template-alignment
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h> class FeatureCloud
{
public:
// A bit of shorthand
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod; FeatureCloud () :
search_method_xyz_ (new SearchMethod),
normal_radius_ (0.02f),
feature_radius_ (0.02f)
{} ~FeatureCloud () {} // Process the given cloud
void
setInputCloud (PointCloud::Ptr xyz)
{
xyz_ = xyz;
processInput ();
} // Load and process the cloud in the given PCD file
void
loadInputCloud (const std::string &pcd_file)
{
xyz_ = PointCloud::Ptr (new PointCloud);
pcl::io::loadPCDFile (pcd_file, *xyz_);
processInput ();
} // Get a pointer to the cloud 3D points
PointCloud::Ptr
getPointCloud () const
{
return (xyz_);
} // Get a pointer to the cloud of 3D surface normals
SurfaceNormals::Ptr
getSurfaceNormals () const
{
return (normals_);
} // Get a pointer to the cloud of feature descriptors
LocalFeatures::Ptr
getLocalFeatures () const
{
return (features_);
} protected:
// Compute the surface normals and local features
void
processInput ()
{
computeSurfaceNormals ();
computeLocalFeatures ();
} // Compute the surface normals
void
computeSurfaceNormals ()
{
normals_ = SurfaceNormals::Ptr (new SurfaceNormals); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
norm_est.setInputCloud (xyz_);
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
} // Compute the local feature descriptors
void
computeLocalFeatures ()
{
features_ = LocalFeatures::Ptr (new LocalFeatures); pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
fpfh_est.setInputCloud (xyz_);
fpfh_est.setInputNormals (normals_);
fpfh_est.setSearchMethod (search_method_xyz_);
fpfh_est.setRadiusSearch (feature_radius_);
fpfh_est.compute (*features_);
} private:
// Point cloud data
PointCloud::Ptr xyz_;
SurfaceNormals::Ptr normals_;
LocalFeatures::Ptr features_;
SearchMethod::Ptr search_method_xyz_; // Parameters
float normal_radius_;
float feature_radius_;
}; class TemplateAlignment
{
public: // A struct for storing alignment results
struct Result
{
float fitness_score;
Eigen::Matrix4f final_transformation;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; TemplateAlignment () :
min_sample_distance_ (0.05f),
max_correspondence_distance_ (0.01f*0.01f),
nr_iterations_ (500)
{
// Initialize the parameters in the Sample Consensus Initial Alignment (SAC-IA) algorithm
sac_ia_.setMinSampleDistance (min_sample_distance_);
sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
sac_ia_.setMaximumIterations (nr_iterations_);
} ~TemplateAlignment () {} // 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 ());
} // Add the given cloud to the list of template clouds
void
addTemplateCloud (FeatureCloud &template_cloud)
{
templates_.push_back (template_cloud);
} // Align the given template cloud to the target specified by 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 ();
} // Align all of template clouds set by addTemplateCloud to the target specified by 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]);
}
} // Align all of template clouds to the target cloud to find the one with best alignment score
int
findBestAlignment (TemplateAlignment::Result &result)
{
// Align all of the templates to the target cloud
std::vector<Result, Eigen::aligned_allocator<Result> > results;
alignAll (results); // Find the template with the best (lowest) fitness score
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;
}
} // Output the best alignment
result = results[best_template];
return (best_template);
} private:
// A list of template clouds and the target to which they will be aligned
std::vector<FeatureCloud> templates_;
FeatureCloud target_; // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
float min_sample_distance_;
float max_correspondence_distance_;
int nr_iterations_;
}; // Align a collection of object templates to a sample point cloud
int
main (int argc, char **argv)
{
if (argc < 3)
{
printf ("No target PCD file given!\n");
return (-1);
} // Load the object templates specified in the object_templates.txt file
std::vector<FeatureCloud> object_templates;
std::ifstream input_stream (argv[1]);
object_templates.resize (0);
std::string pcd_filename;
while (input_stream.good ())
{
std::getline (input_stream, pcd_filename);
if (pcd_filename.empty () || pcd_filename.at (0) == '#') // Skip blank lines or comments
continue; FeatureCloud template_cloud;
template_cloud.loadInputCloud (pcd_filename);
object_templates.push_back (template_cloud);
}
input_stream.close (); // Load the target cloud PCD file
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile (argv[2], *cloud); // Preprocess the cloud by...
// ...removing distant points
const float depth_limit = 1.0;
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, depth_limit);
pass.filter (*cloud); // ... and downsampling the point cloud
const float voxel_grid_size = 0.005f;
pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
vox_grid.setInputCloud (cloud);
vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
//vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html
pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud (new pcl::PointCloud<pcl::PointXYZ>);
vox_grid.filter (*tempCloud);
cloud = tempCloud; // Assign to the target FeatureCloud
FeatureCloud target_cloud;
target_cloud.setInputCloud (cloud); // Set the TemplateAlignment inputs
TemplateAlignment template_align;
for (size_t i = 0; i < object_templates.size (); ++i)
{
template_align.addTemplateCloud (object_templates[i]);
}
template_align.setTargetCloud (target_cloud); // Find the best template alignment
TemplateAlignment::Result best_alignment;
int best_index = template_align.findBestAlignment (best_alignment);
const FeatureCloud &best_template = object_templates[best_index]; // Print the alignment fitness score (values less than 0.00002 are good)
printf ("Best fitness score: %f\n", best_alignment.fitness_score); // Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation = best_alignment.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)); // Save the aligned template for visualization
pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud); return (0);
}
结果查看
pcl_viewer_debug.exe person.pcd output.pcd
PCL 3维点云的模板匹配的更多相关文章
- Halcon编程-基于形状特征的模板匹配
halcon软件最高效的一个方面在于模板匹配,号称可以快速进行柔性模板匹配,能够非常方便的用于缺陷检测.目标定位.下面以一个简单的例子说明基于形状特征的模板匹配. 为了在右图中,定位图中的三 ...
- 模式识别之ocr项目---(模板匹配&BP神经网络训练)
摘 要 在MATLAB环境下利用USB摄像头采集字符图像,读取一帧保存为图像,然后对读取保存的字符图像,灰度化,二值化,在此基础上做倾斜矫正,对矫正的图像进行滤波平滑处理,然后对字符区域进行提取分割出 ...
- 字符识别OCR研究一(模板匹配&BP神经网络训练)
摘 要 在MATLAB环境下利用USB摄像头採集字符图像.读取一帧保存为图像.然后对读取保存的字符图像,灰度化.二值化,在此基础上做倾斜矫正.对矫正的图像进行滤波平滑处理,然后对字符区域进行提取切割出 ...
- OpenCV中的模板匹配/Filter2d
1.模板匹配 模板匹配是在图像中寻找目标的方法之一.Come On, Boy.我们一起来看看模板匹配到底是怎么回事. 参考链接:http://www.opencv.org.cn/opencvdoc/2 ...
- OpenCV笔记(3)(Canny边缘检测、高斯金字塔、拉普拉斯金字塔、图像轮廓、模板匹配)
一.Canny边缘检测 Canny边缘检测是一系列方法综合的结果.其中主要包含以下步骤: 1.使用高斯滤波器,平滑图像,滤除噪声. 2.计算图像中每个像素点的梯度强度和方向. 3.应用非极大值抑制(N ...
- 使用OpenCV&&C++进行模板匹配.
一:课程介绍 1.1:学习目标 学会用imread载入图像,和imshow输出图像. 用nameWindow创建窗口,用createTrackbar加入滚动条和其回调函数的写法. 熟悉OpenCV函数 ...
- halcon三种模板匹配方法
halcon有三种模板匹配方法:即Component-Based.Gray-Value-Based.Shaped_based,分别是基于组件(或成分.元素)的匹配,基于灰度值的匹配和基于形状的匹配,此 ...
- opencv 模板匹配与滑动窗口(单匹配) (多匹配)
1单匹配: 测试图片: code: #include <opencv\cv.h> #include <opencv\highgui.h> #include <open ...
- opencv 在工业中的应用:模板匹配
模板匹配在工业中经常有两个用途,一模板匹配进行产品定位,二根据匹配度来判断是OK的产品还是NG的产品.我用OPENCV做了个模板匹配定位的DEMO. (1)点击打开图像按钮打开一幅图像 (2)点击定义 ...
随机推荐
- 提高ASP.NET页面载入速度的方法
前言 本文是我对ASP.NET页面载入速度提高的一些做法,这些做法分为以下部分: 目录 1.采用 HTTP Module 控制页面的生命周期. 2.自定义Response.Filter得到输出流str ...
- 全面剖析Smarty缓存机制一[三种缓存方式]
今天主要全面总结下Smarty模板引擎中强大的缓存机制,缓存机制有效减少了系统对服务器的压力,而这也是很多开发者喜欢Smarty的原因之一,由于篇幅较大,便于博友阅读,这篇文章将剖析Smarty缓存的 ...
- String.valueof;和String = ""+1;的区别
关于字符串的+操作,单纯的String s ="" +11;编译器会看做常量""和常量11的拼接操作,常量计算最快:String.valueOf会调用方法,速 ...
- 【解决Jira】Chrome提示Java插件因过期而遭到阻止(JIRA上传截屏截图)
最近经常被这个问题所困扰:用Chrome访问JIRA上传截屏截图时,地址栏下面弹出通知,提示JAVA插件已过期.但是由于公司要求统一开发环境和设置,不能更新到最新版,就像这样: 结果网页上的Java就 ...
- WCF揭秘学习笔记(2):数据表示
背景知识 WCF提供了一种语言为软件通信建模,称作服务模型.使用更底层的编程架构提供的类可以从这种语言建立的模型中生成可用的通信软件. 在服务模型使用的语言中,负责通信的软件部分称为服务(servic ...
- ThinkJava-压缩
尽管存在许多种压缩算恙,但是Zip和GZIP可能是最常用的.因此我们可以很容易地使用多 种可读写这些格式的工具来操纵我们的压缩数据. 1 用GZIP进行简单压缩 GZIP接口非常简单, 因此如果我 ...
- Regenerate Script 重置脚本
1.Regenerate Script 重置回录制后的第一次脚本,当修改设定后点击这个按钮,新的设置也会录制到 如:开始没有录到下载的文件,添加下载文件的个时候,再次点击重置,就录制到了 如:如开始是 ...
- [转]Android-Studio 常用配置项
更多内容可参考:http://www.cnblogs.com/smyhvae/p/4390905.html 1. 想要和 Eclipse 一样,Alt + / 出现自动提示列表:在如下位置修改 2. ...
- 自动手动随便你 Win7驱动程序安装自己设
Win7系统是非常智能方便的操作系统,可以自动安装硬件驱动程序,为用户提供了很多方便.但是并不是所有的驱动程序和硬件都能完美兼容,如果不合适就需要卸载了重新安装:还有一些朋友就习惯自己安装驱动,那么, ...
- 在 Laravel 5 中使用 Laravel Excel 实现 Excel/CSV 文件导入导出功能
1.简介 Laravel Excel 在 Laravel 5 中集成 PHPOffice 套件中的 PHPExcel ,从而方便我们以优雅的.富有表现力的代码实现Excel/CSV文件的导入和 导出 ...