一、SIFT 3D关键点检测
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/sift_keypoint.h>//导入SIFT
using namespace std;
// 基于Z梯度估计3D点云的SIFT关键点
namespace pcl
{
template<>
struct SIFTKeypointFieldSelector<PointXYZ>
{
inline float
operator () (const PointXYZ& p) const
{
return p.z;
}
};
}
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::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZ>);
//SIFT
//pcl::StopWatch watch; // 计时器
//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
pcl::PointCloud<pcl::PointWithScale> result1;
pcl::PointCloud<pcl::PointWithScale> result2;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
//-----------------------------SIFT算法参数----------------------------------
const float min_scale = 5.f; // 设置尺度空间中最小尺度的标准偏差
const int n_octaves = 3; // 设置尺度空间层数,越小则特征点越多
const int n_scales_per_octave = 15; // 设置尺度空间中计算的尺度个数
const float min_contrast = 0.01f; // 设置限制关键点检测的阈值
//----------------------------SIFT关键点检测---------------------------------
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift1;//创建sift关键点检测对象)))
sift1.setInputCloud(cloud); // 设置输入点云
sift1.setSearchMethod(tree); // 创建一个空的kd树对象tree,并把它传递给sift检测对象
sift1.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
sift1.setMinimumContrast(min_contrast); // 设置限制关键点检测的阈值
sift1.compute(result1); // 执行sift关键点检测,保存结果在result
cout << "输入点云提取的关键点:" << result1.size() << " keypoints" << endl;
//cout << "SIFT关键点提取用时: " << watch.getTimeSeconds() << "秒" << endl;
copyPointCloud(result1, *keypoints1);
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift2;//创建sift关键点检测对象)))
sift2.setInputCloud(cloud_target); // 设置输入点云
sift2.setSearchMethod(tree); // 创建一个空的kd树对象tree,并把它传递给sift检测对象
sift2.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
sift2.setMinimumContrast(min_contrast); // 设置限制关键点检测的阈值
sift2.compute(result2); // 执行sift关键点检测,保存结果在result
cout << "输出点云提取的关键点:" << result2.size() << " keypoints" << endl;
//cout << "SIFT关键点提取用时: " << watch.getTimeSeconds() << "秒" << endl;
copyPointCloud(result2, *keypoints2);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("v2"));
viewer1->setBackgroundColor(0, 0, 0);
viewer1->setWindowName("sift");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloud, 0.0, 255, 0.0);
viewer1->addPointCloud<pcl::PointXYZ>(keypoints1, single_color1, "key cloud");//特征点
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");
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("sift");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud, 0.0, 255, 0.0);
viewer2->addPointCloud<pcl::PointXYZ>(keypoints2, single_color2, "key cloud");//特征点
viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");
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("sift");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloud, 0.0, 255, 0.0);
viewer3->addPointCloud<pcl::PointXYZ>(cloud, single_color3, "sample cloud");
viewer3->addPointCloud<pcl::PointXYZ>(keypoints1, "key cloud");//特征点
viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");
viewer3->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key cloud");
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("sift");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color4(cloud, 0.0, 255, 0.0);
viewer4->addPointCloud<pcl::PointXYZ>(cloud_target, single_color4, "sample cloud");
viewer4->addPointCloud<pcl::PointXYZ>(keypoints2, "key cloud");//特征点
viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");
viewer4->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key cloud");
while (!viewer4->wasStopped())
{
viewer4->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
return 0;
}
关键代码解析:
// 基于Z梯度估计3D点云的SIFT关键点
namespace pcl
{
template<>
struct SIFTKeypointFieldSelector<PointXYZ>
{
inline float
operator () (const PointXYZ& p) const
{
return p.z;
}
};
}
-
namespace pcl { }
:这个代码块定义了一个命名空间pcl
,其中包含了一些PCL(点云库)的相关功能。 -
template<> struct SIFTKeypointFieldSelector<PointXYZ>
:这是一个模板特化,它告诉编译器对于PointXYZ
类型的点云,如何选择用于SIFT关键点检测的字段。 -
inline float operator () (const PointXYZ& p) const { return p.z; }
:这是一个重载的函数调用运算符,它接受一个PointXYZ
类型的参数并返回一个浮点数。在这种情况下,它选择了点云中每个点的 Z 坐标作为特征值,即特征点的梯度值。
通过这段代码,我们告诉SIFT算法应该根据点云中每个点的Z坐标来选择特征点,而不是使用默认的X或Y坐标。这种方式可能会在垂直方向上更好地捕获点云的结构,因为Z坐标通常代表了点云中的高度信息。
对于这段代码的参数设置,主要关注的是选择适当的特征点字段。在这个例子中,我们选择了Z坐标作为特征值,但根据具体的应用场景,你可以选择其他字段,比如强度、法向量等。选择合适的字段会影响到SIFT算法检测到的特征点的质量和数量。
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZ>);
//SIFT
//pcl::StopWatch watch; // 计时器
//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
pcl::PointCloud<pcl::PointWithScale> result1;
pcl::PointCloud<pcl::PointWithScale> result2;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
//-----------------------------SIFT算法参数----------------------------------
const float min_scale = 5.f; // 设置尺度空间中最小尺度的标准偏差
const int n_octaves = 3; // 设置尺度空间层数,越小则特征点越多
const int n_scales_per_octave = 15; // 设置尺度空间中计算的尺度个数
const float min_contrast = 0.01f; // 设置限制关键点检测的阈值
//----------------------------SIFT关键点检测---------------------------------
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift1;//创建sift关键点检测对象)))
sift1.setInputCloud(cloud); // 设置输入点云
sift1.setSearchMethod(tree); // 创建一个空的kd树对象tree,并把它传递给sift检测对象
sift1.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
sift1.setMinimumContrast(min_contrast); // 设置限制关键点检测的阈值
sift1.compute(result1); // 执行sift关键点检测,保存结果在result
cout << "输入点云提取的关键点:" << result1.size() << " keypoints" << endl;
//cout << "SIFT关键点提取用时: " << watch.getTimeSeconds() << "秒" << endl;
copyPointCloud(result1, *keypoints1);
-
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>);
:创建一个指向包含检测到的关键点的点云的指针。 -
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZ>);
:创建另一个指向点云的指针,用于存储另一组检测到的关键点。 -
pcl::PointCloud<pcl::PointWithScale> result1;
和pcl::PointCloud<pcl::PointWithScale> result2;
:创建两个点云对象,用于存储SIFT算法检测到的特征点,其中pcl::PointWithScale
包含了点的三维坐标以及尺度信息。 -
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
:创建一个KD树搜索对象的指针,用于在点云中搜索最近邻点。 -
const float min_scale = 5.f;
、const int n_octaves = 3;
、const int n_scales_per_octave = 15;
、const float min_contrast = 0.01f;
:定义了SIFT算法的参数。这些参数影响着特征点的检测结果。min_scale
:设置尺度空间中最小尺度的标准偏差,即特征点的最小尺度。n_octaves
:设置尺度空间层数,即金字塔的层数,层数越大可以检测到尺度更大范围的特征。n_scales_per_octave
:设置尺度空间中计算的尺度个数,影响特征点的密度。min_contrast
:设置限制关键点检测的阈值,用于确定哪些特征点被认为是有效的。
-
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift1;
:创建一个SIFT关键点检测对象,指定输入点的类型为pcl::PointXYZ
,输出点的类型为pcl::PointWithScale
。 -
sift1.setInputCloud(cloud);
:设置输入点云,即要在其上执行SIFT关键点检测的点云。 -
sift1.setSearchMethod(tree);
:设置SIFT算法中使用的搜索方法为KD树搜索。 -
sift1.setScales(min_scale, n_octaves, n_scales_per_octave);
:设置SIFT算法中搜索关键点的尺度范围。 -
sift1.setMinimumContrast(min_contrast);
:设置SIFT算法中限制关键点检测的阈值。 -
sift1.compute(result1);
:执行SIFT关键点检测,并将检测到的关键点保存在result1
中。 -
cout << "输入点云提取的关键点:" << result1.size() << " keypoints" << endl;
:输出检测到的关键点数量。 -
copyPointCloud(result1, *keypoints1);
:将检测到的关键点从result1
复制到keypoints1
中,转换为普通的pcl::PointXYZ
类型的点。
这些参数的设置会影响SIFT算法检测到的特征点的数量和质量。例如,调整最小尺度、尺度空间层数和尺度个数会影响特征点的分布和密度,而调整最小对比度阈值会影响检测到的特征点的质量。
结果:
输入点云的关键点
输出点云的关键点
输入点云的关键点与输入点云一起展示
输出点云的关键点与输出点云一起展示
二、SIFT 3D关键点检测及SAC-IA粗配准
C++
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/iss_3d.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>
//导入SIFT
#include <pcl/keypoints/sift_keypoint.h>
using namespace std;
// 基于Z梯度估计3D点云的SIFT关键点
namespace pcl
{
template<>
struct SIFTKeypointFieldSelector<PointXYZ>
{
inline float
operator () (const PointXYZ& p) const
{
return p.z;
}
};
}
void extract_keypoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{
//SIFT
pcl::StopWatch watch; // 计时器
//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
pcl::PointCloud<pcl::PointWithScale> result;
//-----------------------------SIFT算法参数----------------------------------
const float min_scale = 5.f; // 设置尺度空间中最小尺度的标准偏差
const int n_octaves = 3; // 设置尺度空间层数,越小则特征点越多
const int n_scales_per_octave = 15; // 设置尺度空间中计算的尺度个数
const float min_contrast = 0.01f; // 设置限制关键点检测的阈值
//----------------------------SIFT关键点检测---------------------------------
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//创建sift关键点检测对象)))
sift.setInputCloud(cloud); // 设置输入点云
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
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
cout << "Extracted " << result.size() << " keypoints" << endl;
//cout << "SIFT关键点提取用时: " << watch.getTimeSeconds() << "秒" << endl;
copyPointCloud(result, *keypoint);
}
pcl::PointCloud<pcl::FPFHSignature33>::Ptr compute_fpfh_feature(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;
n.setInputCloud(keypoint);
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.setNumberOfThreads(8);
f.setInputCloud(keypoint);
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->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(s_k);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr tk_fpfh = compute_fpfh_feature(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("v1"));
viewer2->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color2(cloud_target, 255, 0, 0);
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博客
和本章第一部分已经解释了大部分函数,这里就不赘述了
结果:
输入点云与输出点云
配准后的输入点云与输出点云,实际效果相对较好,运行不算慢,只要一两分钟就能出结果