一、SUSAN关键点检测
C++
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common_headers.h>
#include <pcl/keypoints/susan.h>
using namespace std;
int main(int, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//要配准变化的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云(不变的)
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1)
{
PCL_ERROR("加载点云失败\n");
}
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1)
{
PCL_ERROR("加载点云失败\n");
}
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZI>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::SUSANKeypoint<pcl::PointXYZ, pcl::PointXYZI> SUSAN1;
SUSAN1.setInputCloud(cloud);
SUSAN1.setSearchMethod(tree);
SUSAN1.setRadius(3.0f); // 设置正常估计和非最大抑制的半径
SUSAN1.setDistanceThreshold(0.01f); // 设置距离阈值
SUSAN1.setAngularThreshold(0.01f); // 设置用于角点检测的角度阈值
SUSAN1.setIntensityThreshold(0.1f); // 设置用于角点检测的强度阈值
SUSAN1.setNonMaxSupression(true); // 对响应应用非最大值抑制,以保持最强角。
SUSAN1.compute(*keypoints1);
pcl::PointCloud<pcl::PointXYZ>::Ptr keys1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*keypoints1, *keys1);
pcl::SUSANKeypoint<pcl::PointXYZ, pcl::PointXYZI> SUSAN2;
SUSAN2.setInputCloud(cloud_target);
SUSAN2.setSearchMethod(tree);
SUSAN2.setRadius(3.0f); // 设置正常估计和非最大抑制的半径
SUSAN2.setDistanceThreshold(0.01f); // 设置距离阈值
SUSAN2.setAngularThreshold(0.01f); // 设置用于角点检测的角度阈值
SUSAN2.setIntensityThreshold(0.1f); // 设置用于角点检测的强度阈值
SUSAN2.setNonMaxSupression(true); // 对响应应用非最大值抑制,以保持最强角。
SUSAN2.compute(*keypoints2);
pcl::PointCloud<pcl::PointXYZ>::Ptr keys2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*keypoints2, *keys2);
关键点显示
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer1(new pcl::visualization::PCLVisualizer("v1"));
viewer1->setBackgroundColor(0, 0, 0);
viewer1->setWindowName("SUSAN");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> in_color1(keys1, 0.0, 255.0, 0.0);
viewer1->addPointCloud<pcl::PointXYZ>(keys1, in_color1, "key_color");//特征点
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key_color");
while (!viewer1->wasStopped())
{
viewer1->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer2(new pcl::visualization::PCLVisualizer("v2"));
viewer2->setBackgroundColor(0, 0, 0);
viewer2->setWindowName("SUSAN");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> in_color2(keys2, 0.0, 255.0, 0.0);
viewer2->addPointCloud<pcl::PointXYZ>(keys2, in_color2, "key_color");//特征点
viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key_color");
while (!viewer2->wasStopped())
{
viewer2->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer3(new pcl::visualization::PCLVisualizer("v3"));
viewer3->setBackgroundColor(0, 0, 0);
viewer3->setWindowName("SUSAN");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> in_color3(cloud, 0.0, 255.0, 0.0);
viewer3->addPointCloud<pcl::PointXYZ>(cloud, in_color3, "in_color");
viewer3->addPointCloud<pcl::PointXYZ>(keys1, "key_color");//特征点
viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key_color");
viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key_color");
while (!viewer3->wasStopped())
{
viewer3->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer4(new pcl::visualization::PCLVisualizer("v4"));
viewer4->setBackgroundColor(0, 0, 0);
viewer4->setWindowName("SUSAN");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> in_color4(cloud_target, 0.0, 255.0, 0.0);
viewer4->addPointCloud<pcl::PointXYZ>(cloud_target, in_color4, "in_color");
viewer4->addPointCloud<pcl::PointXYZ>(keys2, "key_color");//特征点
viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key_color");
viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key_color");
while (!viewer4->wasStopped())
{
viewer4->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
return 0;
}
关键代码解析:
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZI>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pcl::SUSANKeypoint<pcl::PointXYZ, pcl::PointXYZI> SUSAN1;
SUSAN1.setInputCloud(cloud);
SUSAN1.setSearchMethod(tree);
SUSAN1.setRadius(3.0f); // 设置正常估计和非最大抑制的半径
SUSAN1.setDistanceThreshold(0.01f); // 设置距离阈值
SUSAN1.setAngularThreshold(0.01f); // 设置用于角点检测的角度阈值
SUSAN1.setIntensityThreshold(0.1f); // 设置用于角点检测的强度阈值
SUSAN1.setNonMaxSupression(true); // 对响应应用非最大值抑制,以保持最强角。
SUSAN1.compute(*keypoints1);
pcl::PointCloud<pcl::PointXYZ>::Ptr keys1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*keypoints1, *keys1);
-
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZI>);
:创建了一个指向存储PointXYZI类型的点云的指针,该点云用于存储检测到的关键点的坐标及其强度信息。 -
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
:创建了一个KD树,用于最近邻搜索。 -
pcl::SUSANKeypoint<pcl::PointXYZ, pcl::PointXYZI> SUSAN1;
:创建了一个SUSAN关键点检测器对象。 -
SUSAN1.setInputCloud(cloud);
:设置输入点云数据。这里假设cloud
是一个pcl::PointCloud<pcl::PointXYZ>
类型的点云数据。 -
SUSAN1.setSearchMethod(tree);
:设置最近邻搜索方法为之前创建的KD树。 -
SUSAN1.setRadius(3.0f);
:设置用于角点检测的半径大小。这个半径决定了在每个点周围的搜索区域的大小,较大的半径可能会导致检测到更多的关键点。 -
SUSAN1.setDistanceThreshold(0.01f);
:设置距离阈值。这个阈值用于过滤掉与周围点距离过远的候选点,较大的值将允许更远的点被考虑为关键点。 -
SUSAN1.setAngularThreshold(0.01f);
:设置角度阈值。这个阈值用于控制在检测角点时考虑的角度范围,较大的值将允许更大的角度范围内的点被检测为关键点。 -
SUSAN1.setIntensityThreshold(0.1f);
:设置强度阈值。这个阈值用于控制在检测角点时考虑的点云强度(或灰度)的范围,较大的值将允许更强或更弱的点被检测为关键点。 -
SUSAN1.setNonMaxSupression(true);
:设置是否对响应应用非最大值抑制,以保持最强的角点。 -
SUSAN1.compute(*keypoints1);
:运行SUSAN关键点检测算法,将检测到的关键点存储在keypoints1
中。 -
pcl::PointCloud<pcl::PointXYZ>::Ptr keys1(new pcl::PointCloud<pcl::PointXYZ>);
:创建了一个新的点云对象,用于存储不带强度信息的关键点。 -
pcl::copyPointCloud(*keypoints1, *keys1);
:将带有强度信息的关键点复制到新的点云对象keys1
中,去除强度信息,得到不带强度的关键点。
参数的变大或变小会直接影响到检测到的关键点的数量、位置和质量。例如,增加半径、距离阈值、角度阈值和强度阈值可能会导致检测到更多的关键点,而减小这些值则会使得更少的点被检测为关键点。
结果:
输入点云的关键点
输出点云的关键点
输入点云的关键点与输入点云一起展示
输出点云的关键点与输出点云一起展示
二、SUSAN关键点检测及SAC-IA粗配准
C++
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/common/common_headers.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/keypoints/susan.h>
using namespace std;
void extract_keypoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{
pcl::SUSANKeypoint<pcl::PointXYZ, pcl::PointXYZI> SUSAN;
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoint1(new pcl::PointCloud<pcl::PointXYZI>());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
SUSAN.setInputCloud(cloud);
SUSAN.setSearchMethod(tree);
SUSAN.setRadius(3.0f); // 设置正常估计和非最大抑制的半径
SUSAN.setDistanceThreshold(0.01f); // 设置距离阈值
SUSAN.setAngularThreshold(0.01f); // 设置用于角点检测的角度阈值
SUSAN.setIntensityThreshold(0.1f); // 设置用于角点检测的强度阈值
SUSAN.setNonMaxSupression(true); // 对响应应用非最大值抑制,以保持最强角。
SUSAN.compute(*keypoint1);
pcl::copyPointCloud(*keypoint1, *keypoint);
}
pcl::PointCloud<pcl::FPFHSignature33>::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
int i = 0;
for (auto p : cloud->points)
{
for (auto k : keypoint->points)
{
if (k.x == p.x && k.y == p.y && k.z == p.z)
{
inliers->indices.push_back(i);
}
}
i++;
}
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
f.setIndices(inliers);
f.setNumberOfThreads(8);
f.setInputCloud(cloud);
f.setInputNormals(normals);
f.setSearchMethod(tree);
f.setRadiusSearch(50);
f.compute(*fpfh);
return fpfh;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sac_align(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr s_k, pcl::PointCloud<pcl::PointXYZ>::Ptr t_k, pcl::PointCloud<pcl::FPFHSignature33>::Ptr sk_fpfh, pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh)
{
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(s_k);
scia.setInputTarget(t_k);
scia.setSourceFeatures(sk_fpfh);
scia.setTargetFeatures(tk_fpfh);
scia.setMinSampleDistance(7);///参数:设置采样点之间的最小距离,满足的被当做采样点
scia.setNumberOfSamples(100);设置每次迭代设置采样点的个数(这个参数多可以增加配准精度)
scia.setCorrespondenceRandomness(6);//设置选择随机特征对应点时要使用的邻域点个数。值越大,特征匹配的随机性就越大
pcl::PointCloud<pcl::PointXYZ>::Ptr sac_result(new pcl::PointCloud<pcl::PointXYZ>);
scia.align(*sac_result);
pcl::transformPointCloud(*cloud, *sac_result, scia.getFinalTransformation());
return sac_result;
}
int main(int, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//要配准变化的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);//目标点云(不变的)
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1)
{
PCL_ERROR("加载点云失败\n");
}
if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1)
{
PCL_ERROR("加载点云失败\n");
}
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer1(new pcl::visualization::PCLVisualizer("v1"));
viewer1->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color1(cloud_target, 255, 0, 0);
viewer1->setWindowName("SUSAN");
viewer1->addPointCloud<pcl::PointXYZ>(cloud_target, target_color1, "target cloud");
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
// 对源点云着色可视化 (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color1(cloud, 0, 255, 0);
viewer1->addPointCloud<pcl::PointXYZ>(cloud, input_color1, "input cloud");
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");
while (!viewer1->wasStopped())
{
viewer1->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
//粗配准
pcl::PointCloud<pcl::PointXYZ>::Ptr s_k(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr t_k(new pcl::PointCloud<pcl::PointXYZ>);
extract_keypoint(cloud, s_k);
extract_keypoint(cloud_target, t_k);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr sk_fpfh = compute_fpfh_feature(cloud, s_k);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh = compute_fpfh_feature(cloud_target, t_k);
pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
result = sac_align(cloud, s_k, t_k, sk_fpfh, tk_fpfh);
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer2(new pcl::visualization::PCLVisualizer("v2"));
viewer2->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color2(cloud_target, 255, 0, 0);
viewer2->setWindowName("SUSAN");
viewer2->addPointCloud<pcl::PointXYZ>(cloud_target, target_color2, "target cloud");
viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
// 对源点云着色可视化 (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color2(result, 0, 255, 0);
viewer2->addPointCloud<pcl::PointXYZ>(result, input_color2, "input cloud");
viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");
while (!viewer2->wasStopped())
{
viewer2->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
return 0;
}
关键代码解析:
我之前在iss关键点检测以及SAC-IA粗配准-CSDN博客
和本章第一部分已经解释了大部分函数,这里就不赘述了
结果:
输入点云与输出点云
配准后的输入点云与输出点云,实际效果相对较好,很快就能出结果
注意:运行的时候用visual studio的Release模式,Debug模式会报错,除非去掉 SUSAN.setNonMaxSupression(true); 才能在Debug运行,但是配准速度会非常慢。