PCL笔记八：关键点
关键点提取是2D和3D信息处理中不可或缺的关键技术。
NARF（Normal Aligned Radial Feature）关键点
NARF（Normal Aligned Radial Feature）关键点是为了从深度图像中识别物体而提出的，关键点探测的重要一步是减少特征提取时的搜索空间，把重点放在重要的结构上，对NARF关键点提取过程中有以下要求：
- 提取的过程必须将边缘以及物体表面变化信息考虑在内；
- 关键点的位置必须稳定，可以被重复探测，及时换了不同的视角；
- 关键点所在的位置必须有稳定的支持区域，可以计算描述子并进行唯一的法向量估计。
关键点探测步骤如下：
- 遍历每个深度图像点，通过寻找在近邻区域有深度突变的位置进行边缘检测。
- 遍历每个深度图像点，根据近邻区域的表面变化决定一种测度表面变化的系数，以及变化的主方向。
- 根据第二步找到的主方向计算兴趣值，表征该方向与其他方向的不同，以及该处表面的变化情况，即改点有多稳定。
- 对兴趣值进行平滑过滤。
- 进行无最大值压缩找到最终的关键点，纪委NARF关键点。
Harris关键点：
Harris关键点检测通过计算图像点的Harris矩阵和矩阵对应的特征值来判断是否为关键点。如果Harris矩阵特征的两个特征值都很大，则该点为关键点。
Harris关键点检测算子对于图像旋转变换保持较好的检测重复率，但不适合尺寸变化的关键点检测。
点云中的3D Harris关键点借鉴了2D Harris关键点检测的思想，不过3D Harris关键点检测使用的是点云表面法向量的信息，而不是2D Harris关键点检测使用的图像梯度。
距离图像中提取NARF关键点
/* \author Bastian Steder */
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include // for getFilenameWithoutExtension
#ifdef WIN32
#define pcl_sleep(x) Sleep(1000*(x))
#else
#define pcl_sleep(x) sleep(x)
#endif
typedef pcl::PointXYZ PointType;
//参数
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
//打印帮助
void
printUsage(const char* progName)
{
std::cout << "\n\nUsage: " << progName << " [options] \n\n"
<< "Options:\n"
<< "-------------------------------------------\n"
<< "-r angular resolution in degrees (default " << angular_resolution << ")\n"
<< "-c coordinate frame (default " << (int)coordinate_frame << ")\n"
<< "-m Treat all unseen points as maximum range readings\n"
<< "-s support size for the interest points (diameter of the used sphere - "
<< "default " << support_size << ")\n"
<< "-h this help\n"
<< "\n\n";
}
void
setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
//viewer.camera_.pos[0] = pos_vector[0];
//viewer.camera_.pos[1] = pos_vector[1];
//viewer.camera_.pos[2] = pos_vector[2];
//viewer.camera_.focal[0] = look_at_vector[0];
//viewer.camera_.focal[1] = look_at_vector[1];
//viewer.camera_.focal[2] = look_at_vector[2];
//viewer.camera_.view[0] = up_vector[0];
//viewer.camera_.view[1] = up_vector[1];
//viewer.camera_.view[2] = up_vector[2];
viewer.setCameraPosition(pos_vector[0], pos_vector[1], pos_vector[2], look_at_vector[0], look_at_vector[1], look_at_vector[2], up_vector[0], up_vector[1], up_vector[2]);
//viewer.updateCamera();
}
// -----Main-----
int
main(int argc, char** argv)
{
//解析命令行参数
if (pcl::console::find_argument(argc, argv, "-h") >= 0)
{
printUsage(argv[0]);
return 0;
}
if (pcl::console::find_argument(argc, argv, "-m") >= 0)
{
setUnseenToMaxRange = true;
cout << "Setting unseen values in range image to maximum range readings.\n";
}
int tmp_coordinate_frame;
if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0)
{
coordinate_frame = pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
cout << "Using coordinate frame " << (int)coordinate_frame << ".\n";
}
if (pcl::console::parse(argc, argv, "-s", support_size) >= 0)
cout << "Setting support size to " << support_size << ".\n";
if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to " << angular_resolution << "deg.\n";
angular_resolution = pcl::deg2rad(angular_resolution);
//读取给定的pcd文件或者自行创建随机点云
pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud);
pcl::PointCloud& point_cloud = *point_cloud_ptr;
pcl::PointCloudfar_ranges;
Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
std::vectorpcd_filename_indices = pcl::console::parse_file_extension_argument(argc, argv, "pcd");
if (!pcd_filename_indices.empty())
{
std::string filename = argv[pcd_filename_indices[0]];
if (pcl::io::loadPCDFile(filename, point_cloud) == -1)
{
cerr << "Was not able to open file \"" << filename << "\".\n";
printUsage(argv[0]);
return 0;
}
scene_sensor_pose = Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2])) *
Eigen::Affine3f(point_cloud.sensor_orientation_);
std::string far_ranges_filename = pcl::getFilenameWithoutExtension(filename) + "_far_ranges.pcd";
if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
std::cout << "Far ranges file \"" << far_ranges_filename << "\" does not exists.\n";
}
else
{
setUnseenToMaxRange = true;
cout << "\nNo *.pcd file given =>Genarating example point cloud.\n\n";
for (float x = -0.5f; x <= 0.5f; x += 0.01f)
{
for (float y = -0.5f; y <= 0.5f; y += 0.01f)
{
PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
point_cloud.points.push_back(point);
}
}
point_cloud.width = (int)point_cloud.points.size(); point_cloud.height = 1;
}
//从点云创建距离图像
float noise_level = 0.0;
float min_range = 0.0f;
int border_size = 1;
pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),
scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
range_image.integrateFarRanges(far_ranges);
if (setUnseenToMaxRange)
range_image.setUnseenToMaxRange();
// 创建3D点云可视化窗口，并显示点云
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustomrange_image_color_handler(range_image_ptr, 0, 0, 0);
viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f);
//PointCloudColorHandlerCustompoint_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters();
setViewerPose(viewer, range_image.getTransformationToWorldSystem());
// 显示距离图像
pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
range_image_widget.showRangeImage(range_image);
//提取NARF关键点
pcl::RangeImageBorderExtractor range_image_border_extractor; // 用来边缘提取。NARF第一步就是需要探测出深度图像的边缘。
pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);
narf_keypoint_detector.setRangeImage(&range_image);
narf_keypoint_detector.getParameters().support_size = support_size;
//narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
//narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
pcl::PointCloudkeypoint_indices;
narf_keypoint_detector.compute(keypoint_indices); // 关键点探测
std::cout << "Found " << keypoint_indices.points.size() << " key points.\n";
//在距离图像显示组件内显示关键点
//for (size_ti=0; i::Ptr keypoints_ptr(new pcl::PointCloud);
pcl::PointCloud& keypoints = *keypoints_ptr;
keypoints.points.resize(keypoint_indices.points.size());
for (size_t i = 0; i < keypoint_indices.points.size(); ++i)
keypoints.points[i].getVector3fMap() = range_image.points[keypoint_indices.points[i]].getVector3fMap();
pcl::visualization::PointCloudColorHandlerCustomkeypoints_color_handler(keypoints_ptr, 0, 255, 0);
viewer.addPointCloud(keypoints_ptr, keypoints_color_handler, "keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
// 主循环
while (!viewer.wasStopped())
{
range_image_widget.spinOnce();// process GUI events
viewer.spinOnce();
pcl_sleep(0.01);
}
}
SIFT关键点提取
SIFT：尺度不变特征变换（Scale-invariant feature transform, SIFT）。局部特征描述子。
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
namespace pcl
{
template<>
struct SIFTKeypointFieldSelector
{
inline float
operator () (const PointXYZ& p) const
{
return p.z;
}
};
}
int
main(int argc, char* argv[])
{
pcl::PointCloud::Ptr cloud_xyz(new pcl::PointCloud);
pcl::io::loadPCDFile(argv[1], *cloud_xyz);
const float min_scale = stof(argv[2]);
const int n_octaves = stof(argv[3]);
const int n_scales_per_octave = stof(argv[4]);
const float min_contrast = stof(argv[5]);
pcl::SIFTKeypoint sift;//创建sift关键点检测对象
pcl::PointCloud result;
sift.setInputCloud(cloud_xyz);//设置输入点云
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
sift.setSearchMethod(tree);//创建一个空的kd树对象tree，并把它传递给sift检测对象
sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围：
sift.setMinimumContrast(min_contrast);//设置限制关键点检测的阈值
sift.compute(result);//执行sift关键点检测，保存结果在result：类型为pcl::PointWithScale。
pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud);
// 为了后期处理与显示的需要。需要将SIFT关键点检测结果转换为点类型为pcl::PointXYZ的数据。
copyPointCloud(result, *cloud_temp);//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
//可视化输入点云和关键点
pcl::visualization::PCLVisualizer viewer("Sift keypoint");
viewer.setBackgroundColor(255, 255, 255);
viewer.addPointCloud(cloud_xyz, "cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "cloud");
viewer.addPointCloud(cloud_temp, "keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 9, "keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 255, "keypoints");
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
return 0;
} SIFT关键点检测队形相关的参数：
setScales函数用于指定搜索关键点的尺度范围。
sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围：
- min_scale：用于设置尺度空间中最小尺度的标准偏差，
- 参数octaves是高斯金字塔中组（Octave）的数目，
- 参数nr_scales_per_octave是每组（Octave）计算的尺度（scale）数目。
Harris关键点提取
既可以提取角点也可以提取边缘点。3D Harris角点检测利用的是点云法向量的信息。
#include
#include
#include
#include
#include
#include //harris特征点估计类头文件声明
#include
#include
#include
using namespace std;
int main(int argc, char* argv[])
{
pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud);
pcl::io::loadPCDFile(argv[1], *input_cloud);
pcl::PCDWriter writer;
float r_normal;
float r_keypoint;
r_normal = stof(argv[2]);
r_keypoint = stof(argv[3]);
typedef pcl::visualization::PointCloudColorHandlerCustom ColorHandlerT3;
// 创建关键点估计对象，用于保存Harris关键点。
pcl::PointCloud::Ptr Harris_keypoints(new pcl::PointCloud());
// Harris特征检测对象
pcl::HarrisKeypoint3D* harris_detector = new pcl::HarrisKeypoint3D;
//harris_detector->setNonMaxSupression(true);
harris_detector->setRadius(r_normal); // 法向量估计的半径。
harris_detector->setRadiusSearch(r_keypoint); // 关键点估计的近邻搜索半径。
harris_detector->setInputCloud(input_cloud);
//harris_detector->setNormals(normal_source);
//harris_detector->setMethod(pcl::HarrisKeypoint3D::LOWE);
harris_detector->compute(*Harris_keypoints);
cout << "Harris_keypoints的大小是" << Harris_keypoints->size() << endl;
writer.write("Harris_keypoints.pcd", *Harris_keypoints, false);
pcl::visualization::PCLVisualizer visu3("clouds");
visu3.setBackgroundColor(255, 255, 255);
visu3.addPointCloud(Harris_keypoints, ColorHandlerT3(Harris_keypoints, 0.0, 0.0, 255.0), "Harris_keypoints");
visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "Harris_keypoints");
visu3.addPointCloud(input_cloud, "input_cloud");
visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "input_cloud");
visu3.spin();
} 设置【项目】【属性】【配置属性】【调试】【命令参数】：room.pcd 0.1 0.1
基于对应点分类的对象识别
对应点聚类算法：利用特征匹配得到场景中的对应点，基于对应点聚类为待识别模型实例。
算法输出的每个聚类，代表场景中的一个模型实例假设，同时，对应一个变换矩阵，是该模型实例假设对应的位姿估计。
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include // class "pcl::UniformSampling" 没有成员 "compute"
typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;
std::string model_filename_;
std::string scene_filename_;
//Algorithm params
bool show_keypoints_(false);
bool show_correspondences_(false);
bool use_cloud_resolution_(false);
bool use_hough_(true);
float model_ss_(0.01f);
float scene_ss_(0.03f);
float rf_rad_(0.015f);
float descr_rad_(0.02f);
float cg_size_(0.01f);
float cg_thresh_(5.0f);
# define pcl_isfinite(x) std::isfinite(x)
void
showHelp(char* filename)
{
std::cout << std::endl;
std::cout << "***************************************************************************" << std::endl;
std::cout << "* *" << std::endl;
std::cout << "* Correspondence Grouping Tutorial - Usage Guide *" << std::endl;
std::cout << "* *" << std::endl;
std::cout << "***************************************************************************" << std::endl << std::endl;
std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
std::cout << "Options:" << std::endl;
std::cout << " -h: Show this help." << std::endl;
std::cout << " -k: Show used keypoints." << std::endl;
std::cout << " -c: Show used correspondences." << std::endl;
std::cout << " -r: Compute the model cloud resolution and multiply" << std::endl;
std::cout << " each radius given by that value." << std::endl;
std::cout << " --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
std::cout << " --model_ss val: Model uniform sampling radius (default 0.01)" << std::endl;
std::cout << " --scene_ss val: Scene uniform sampling radius (default 0.03)" << std::endl;
std::cout << " --rf_rad val: Reference frame radius (default 0.015)" << std::endl;
std::cout << " --descr_rad val: Descriptor radius (default 0.02)" << std::endl;
std::cout << " --cg_size val: Cluster size (default 0.01)" << std::endl;
std::cout << " --cg_thresh val: Clustering threshold (default 5)" << std::endl << std::endl;
}
void
parseCommandLine(int argc, char* argv[])
{
//Show help
if (pcl::console::find_switch(argc, argv, "-h"))
{
showHelp(argv[0]);
exit(0);
}
//Model & scene filenames
std::vector filenames;
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
if (filenames.size() != 2)
{
std::cout << "Filenames missing.\n";
showHelp(argv[0]);
exit(-1);
}
model_filename_ = argv[filenames[0]];
scene_filename_ = argv[filenames[1]];
//Program behavior
if (pcl::console::find_switch(argc, argv, "-k"))
{
show_keypoints_ = true;
}
if (pcl::console::find_switch(argc, argv, "-c"))
{
show_correspondences_ = true;
}
if (pcl::console::find_switch(argc, argv, "-r"))
{
// 所有参数将于点云分辨率相乘，得到最终使用的参数。
use_cloud_resolution_ = true;
}
std::string used_algorithm;
if (pcl::console::parse_argument(argc, argv, "--algorithm", used_algorithm) != -1)
{
if (used_algorithm.compare("Hough") == 0)
{
use_hough_ = true;
}
else if (used_algorithm.compare("GC") == 0)
{
use_hough_ = false;
}
else
{
std::cout << "Wrong algorithm name.\n";
showHelp(argv[0]);
exit(-1);
}
}
//General parameters
pcl::console::parse_argument(argc, argv, "--model_ss", model_ss_);
pcl::console::parse_argument(argc, argv, "--scene_ss", scene_ss_);
pcl::console::parse_argument(argc, argv, "--rf_rad", rf_rad_);
pcl::console::parse_argument(argc, argv, "--descr_rad", descr_rad_);
pcl::console::parse_argument(argc, argv, "--cg_size", cg_size_);
pcl::console::parse_argument(argc, argv, "--cg_thresh", cg_thresh_);
}
// 计算点云的空间分辨率：算出输入点云的每个点与其临近点距离的平均值。
double
computeCloudResolution(const pcl::PointCloud::ConstPtr& cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector indices(2);
std::vector sqr_distances(2);
pcl::search::KdTree tree;
tree.setInputCloud(cloud);
for (size_t i = 0; i < cloud->size(); ++i)
{
if (!pcl_isfinite((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt(sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}
int
main(int argc, char* argv[])
{
parseCommandLine(argc, argv);
pcl::PointCloud::Ptr model(new pcl::PointCloud());
pcl::PointCloud::Ptr model_keypoints(new pcl::PointCloud());
pcl::PointCloud::Ptr scene(new pcl::PointCloud());
pcl::PointCloud::Ptr scene_keypoints(new pcl::PointCloud());
pcl::PointCloud::Ptr model_normals(new pcl::PointCloud());
pcl::PointCloud::Ptr scene_normals(new pcl::PointCloud());
pcl::PointCloud::Ptr model_descriptors(new pcl::PointCloud());
pcl::PointCloud::Ptr scene_descriptors(new pcl::PointCloud());
//
// Load clouds
//
if (pcl::io::loadPCDFile(model_filename_, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
showHelp(argv[0]);
return (-1);
}
if (pcl::io::loadPCDFile(scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp(argv[0]);
return (-1);
}
//
// Set up resolution invariance
//
if (use_cloud_resolution_)
{
float resolution = static_cast (computeCloudResolution(model));
if (resolution != 0.0f)
{
model_ss_ *= resolution;
scene_ss_ *= resolution;
rf_rad_ *= resolution;
descr_rad_ *= resolution;
cg_size_ *= resolution;
}
std::cout << "Model resolution: " << resolution << std::endl;
std::cout << "Model sampling size: " << model_ss_ << std::endl;
std::cout << "Scene sampling size: " << scene_ss_ << std::endl;
std::cout << "LRF support radius: " << rf_rad_ << std::endl;
std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
std::cout << "Clustering bin size: " << cg_size_ << std::endl << std::endl;
}
//
// Compute Normals
// 计算场景和模型的每一个点的法向量，
pcl::NormalEstimationOMP norm_est;
norm_est.setKSearch(10); // 计算法向量时，对于每一个点利用10个临近点，
// 该临近点设置的数量是一个经验值，已被证实可以较好的应用于Kinect等获取的数据分辨率;但对于非常稠密的点云数据来说，可以用近似半径来控制 或设置其他参数，很大程度上取决于数据。
norm_est.setInputCloud(model);
norm_est.compute(*model_normals);
norm_est.setInputCloud(scene);
norm_est.compute(*scene_normals);
//
// Downsample Clouds to Extract keypoints
// 为了获取较为稀疏的关键点，下采样，
pcl::PointCloud sampled_indices;
pcl::UniformSampling uniform_sampling;
uniform_sampling.setInputCloud(model);
uniform_sampling.setRadiusSearch(model_ss_); // 均匀采样的半径：默认值：0.03
//uniform_sampling.compute(sampled_indices);
//pcl::copyPointCloud(*model, sampled_indices.points, *model_keypoints);
uniform_sampling.filter(*model_keypoints);
std::cout << "Model total points: " << model->size() << "; Selected Keypoints: " << model_keypoints->size() << std::endl;
uniform_sampling.setInputCloud(scene);
uniform_sampling.setRadiusSearch(scene_ss_);
//uniform_sampling.compute(sampled_indices);
//pcl::copyPointCloud(*scene, sampled_indices.points, *scene_keypoints);
uniform_sampling.filter(*scene_keypoints);
std::cout << "Scene total points: " << scene->size() << "; Selected Keypoints: " << scene_keypoints->size() << std::endl;
//
// Compute Descriptor for keypoints
// 为场景和模型的每个关键点建立特征描述子，计算每个模型和场景的关键点的3D描述子。SHOT描述子。
pcl::SHOTEstimationOMP descr_est;
descr_est.setRadiusSearch(descr_rad_); // 描述子描述区域范围大小。
descr_est.setInputCloud(model_keypoints);
descr_est.setInputNormals(model_normals);
descr_est.setSearchSurface(model);
descr_est.compute(*model_descriptors);
descr_est.setInputCloud(scene_keypoints);
descr_est.setInputNormals(scene_normals);
descr_est.setSearchSurface(scene);
descr_est.compute(*scene_descriptors);
// 利用KD树结构找到模型和场景的对应点。
// Find Model-Scene Correspondences with KdTree
//模型和场景描述子点云之间对应点对集合。
pcl::CorrespondencesPtr model_scene_corrs(new pcl::Correspondences());
// 构造模型描述子点云的KdTreeFLANN，
pcl::KdTreeFLANN match_search;
match_search.setInputCloud(model_descriptors);
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.// 最近邻模型描述子 添加到 对应场景描述子 的 向量中。
for (size_t i = 0; i < scene_descriptors->size(); ++i)
{
std::vector neigh_indices(1);
std::vector neigh_sqr_dists(1);
if (!pcl_isfinite(scene_descriptors->at(i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch(scene_descriptors->at(i), 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
// 当描述子平均距离小于0.25,添加匹配点，SHOT描述子本身设计使其距离保持在0到1之间。
{
pcl::Correspondence corr(neigh_indices[0], static_cast (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back(corr);
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size() << std::endl;
//
// Actual Clustering聚类。
//
std::vector > rototranslations;
std::vector clustered_corrs;
// Using Hough3D Hough投票过程。
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
// 利用Hough算法，需要计算关键点的局部参考坐标系。
pcl::PointCloud::Ptr model_rf(new pcl::PointCloud());
pcl::PointCloud::Ptr scene_rf(new pcl::PointCloud());
// 局部参考坐标系LRF。
pcl::BOARDLocalReferenceFrameEstimation rf_est;
rf_est.setFindHoles(true);
rf_est.setRadiusSearch(rf_rad_); // 估计局部参考坐标系时，当前点的领域搜索半径。
rf_est.setInputCloud(model_keypoints);
rf_est.setInputNormals(model_normals);
rf_est.setSearchSurface(model);
rf_est.compute(*model_rf);
rf_est.setInputCloud(scene_keypoints);
rf_est.setInputNormals(scene_normals);
rf_est.setSearchSurface(scene);
rf_est.compute(*scene_rf);
// Clustering聚类。
pcl::Hough3DGrouping clusterer;
clusterer.setHoughBinSize(cg_size_); // Hough空间的采样间隔
clusterer.setHoughThreshold(cg_thresh_); // Hough空间确定是否有实例存在的最少票数阈值。
clusterer.setUseInterpolation(true); // 是否对投票在Hough空间进行插值计算。
clusterer.setUseDistanceWeight(false); // 在投票时是否将对应点之间的距离作为权重参与计算。
clusterer.setInputCloud(model_keypoints);
clusterer.setInputRf(model_rf); // 设置模型对应的LRF
clusterer.setSceneCloud(scene_keypoints);
clusterer.setSceneRf(scene_rf); // 设置场景对应的LRF
clusterer.setModelSceneCorrespondences(model_scene_corrs); // 设置模型与场景的对应点对集合。
//clusterer.cluster (clustered_corrs);
clusterer.recognize(rototranslations, clustered_corrs); // 结果包含 变换矩阵 和对应点 聚类 结果。
}
else // Using GeometricConsistency // 使用几何一致性聚类
{
pcl::GeometricConsistencyGrouping gc_clusterer; // 算法实例。
gc_clusterer.setGCSize(cg_size_); // 检查几何一致性时的空间分辨率。
gc_clusterer.setGCThreshold(cg_thresh_); // 设置最小的聚类数量。
gc_clusterer.setInputCloud(model_keypoints); // 设置模型关键点。
gc_clusterer.setSceneCloud(scene_keypoints); // 设置场景关键点。
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize(rototranslations, clustered_corrs);// 结果包含 变换矩阵 和对应点 聚类 结果。
}
//
// Output results
// 识别算法返回一个Eigen::Matrix4f类型的矩阵向量，该矩阵向量代表场景中找到模型的每个实例的变换矩阵（旋转矩阵 + 平移向量）；
// 识别算法还返回对应的支持每个模型实例的对应点对聚类，以向量形式保存，该向量的每个元素依次都是对应点对的集合，这些集合代表与场景中具体实例相关联的对应点。
std::cout << "Model instances found: " << rototranslations.size() << std::endl;
for (size_t i = 0; i < rototranslations.size(); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size() << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3, 3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].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));
}
//
// Visualization
//
pcl::visualization::PCLVisualizer viewer("点云库PCL学习教程第二版-基于对应点聚类的3D模型识别");
viewer.addPointCloud(scene, "scene_cloud");
viewer.setBackgroundColor(255, 255, 255);
pcl::PointCloud::Ptr off_scene_model(new pcl::PointCloud());
pcl::PointCloud::Ptr off_scene_model_keypoints(new pcl::PointCloud());
if (show_correspondences_ || show_keypoints_)
{
// We are translating the model so that it doesn't end in the middle of the scene representation
pcl::transformPointCloud(*model, *off_scene_model, Eigen::Vector3f(0, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
pcl::transformPointCloud(*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f(0, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
pcl::visualization::PointCloudColorHandlerCustom off_scene_model_color_handler(off_scene_model, 0, 255, 0);
viewer.addPointCloud(off_scene_model, off_scene_model_color_handler, "off_scene_model");
}
if (show_keypoints_)
{
pcl::visualization::PointCloudColorHandlerCustom scene_keypoints_color_handler(scene_keypoints, 0, 0, 255);
viewer.addPointCloud(scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
pcl::visualization::PointCloudColorHandlerCustom off_scene_model_keypoints_color_handler(off_scene_model_keypoints, 0, 0, 255);
viewer.addPointCloud(off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
}
for (size_t i = 0; i < rototranslations.size(); ++i)
{
pcl::PointCloud::Ptr rotated_model(new pcl::PointCloud());
pcl::transformPointCloud(*model, *rotated_model, rototranslations[i]);
std::stringstream ss_cloud;
ss_cloud << "instance" << i;
pcl::visualization::PointCloudColorHandlerCustom rotated_model_color_handler(rotated_model, 255, 0, 0);
viewer.addPointCloud(rotated_model, rotated_model_color_handler, ss_cloud.str());
if (show_correspondences_)
{
for (size_t j = 0; j < clustered_corrs[i].size(); ++j)
{
std::stringstream ss_line;
ss_line << "correspondence_line" << i << "_" << j;
PointType& model_point = off_scene_model_keypoints->at(clustered_corrs[i][j].index_query);
PointType& scene_point = scene_keypoints->at(clustered_corrs[i][j].index_match);
// We are drawing a line for each pair of clustered correspondences found between the model and the scene
viewer.addLine(model_point, scene_point, 0, 255, 0, ss_line.str());
}
}
}
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
return (0);
} 需要在
添加参数如下：
milk_pose_changed.pcd milk_cartoon_all_small_clorox.pcd -k -c
bug记录。