RANSAC（Random Sample Consensus）和ICP（Iterative Closest Point）是点云配准中常用的两个算法，可以使用RANSAC进行粗配准结合ICP进行精配准。
1.FPFH特征
-
FPFH特征：
- 定义：FPFH是一种点云特征描述子，用于表示点云中每个点的局部特征。
- 计算过程：
- 对于每个点，计算其法向量（通常使用最近邻点法）。
- 选择一个半径内的邻域（例如，以该点为中心的球体）。
- 对于每个邻域中的点，计算其与中心点的相对位置和法向量之间的关系。
- 构建直方图，将这些相对位置和法向量的关系编码为特征向量。
- 特点：
- FPFH保留了PFH（Point Feature Histogram）的信息，但计算效率更高。
- 它对点云的旋转、平移和尺度变换具有一定的不变性。
-
核心思想：
- FPFH的核心思想是将每个点的局部几何信息编码为一个紧凑的特征向量，以便在点云匹配和配准中使用。
- 通过计算点与其邻域中其他点的关系，FPFH能够捕捉到点云的表面形状、曲率和法向量信息。
-
应用：
- FPFH特征通常用于点云配准的初始阶段，例如RANSAC中的粗配准。
- 它可以帮助识别具有相似局部几何特征的点，从而估计初始的变换矩阵。
2.整体流程
-
FPFH特征计算：
- 对于每个点，选择一个半径内的邻域（例如，以该点为中心的球体）。
- 对于邻域中的每个点，计算其与中心点的相对位置和法向量之间的关系。
- 在RANSAC中，我们使用FPFH特征来识别具有相似局部几何特征的点对。
- 通过匹配FPFH特征，我们可以估计初始的变换矩阵，将源点云变换到目标点云的坐标系中。
-
RANSAC算法（粗配准）：
- 随机选择源点云中的一组点作为初始匹配点对。
- 在目标点云中查找与这组点最接近的点。
- 计算两个三角形之间的变换矩阵（通常使用最小二乘法）。
- 重复上述步骤，维护并返回重合度最高的变换。
-
ICP算法（精配准）：
- 使用RANSAC得到的初始变换矩阵。
- 迭代优化变换矩阵，使点云对齐。
- ICP通过最小化点云之间的距离来优化变换矩阵。
-
迭代：
- 如果匹配的阈值不满足要求，继续迭代RANSAC和ICP。
- 在每次迭代中，更新匹配点对和变换矩阵。
3.相关程序
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/3dsc.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/random_sample.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>
#include <vector>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <Eigen/Dense>
#include<cmath>
#include <pcl/registration/registration.h>
# define pcl_isfinite(x) std::isfinite(x)
using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr input_cloud)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
//-------------------------法向量估计-----------------------
pointnormal::Ptr normals(new pointnormal);
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
n.setInputCloud(input_cloud);
n.setNumberOfThreads(8);
n.setSearchMethod(tree);
n.setKSearch(10);
//n.setRadiusSearch(0.01);
n.compute(*normals);
//-------------------------FPFH估计-------------------------
fpfhFeature::Ptr fpfh(new fpfhFeature);
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fest;
fest.setNumberOfThreads(8);
fest.setInputCloud(input_cloud);
fest.setInputNormals(normals);
fest.setSearchMethod(tree);
fest.setKSearch(10);
//fest.setRadiusSearch(0.025);
fest.compute(*fpfh);
return fpfh;
}
int main(int argc, char** argv)
{
pcl::PointCloud<PointT>::Ptr source(new pcl::PointCloud<PointT>);
pcl::io::loadPLYFile("C:/Users/17927/Desktop/QYQ/D1-2.ply", *source);
pcl::PointCloud<PointT>::Ptr target(new pcl::PointCloud<PointT>);
pcl::io::loadPLYFile("C:/Users/17927/Desktop/QYQ/D1-1.ply", *target);
clock_t start = clock();
//---------------计算源点云和目标点云的FPFH----------------------
fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source);
fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target);
//-------------------------SAC配准--------------------------------------------------
pcl::SampleConsensusPrerejective<PointT, PointT, pcl::FPFHSignature33> r_sac;
r_sac.setInputSource(source);
r_sac.setInputTarget(target);
r_sac.setSourceFeatures(source_fpfh);
r_sac.setTargetFeatures(target_fpfh);
r_sac.setCorrespondenceRandomness(5);
r_sac.setInlierFraction(0.5f);
r_sac.setNumberOfSamples(3);
r_sac.setSimilarityThreshold(0.1f);
r_sac.setMaxCorrespondenceDistance(1.0f);
r_sac.setMaximumIterations(100);
pointcloud::Ptr align(new pointcloud);
r_sac.align(*align);
Eigen::Matrix4f sac_trans;
sac_trans = r_sac.getFinalTransformation();
std::cout << sac_trans << endl;
clock_t sac_time = clock();
//-----------------------icp配准-------------------------------------------
pcl::PointCloud<PointT>::Ptr icp_result(new pcl::PointCloud<PointT>);
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(source);
icp.setInputTarget(target);
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(35);
icp.setTransformationEpsilon(1e-10);
icp.setEuclideanFitnessEpsilon(0.01);
icp.align(*icp_result, sac_trans);
clock_t end = clock();
cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << endl;
Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
cout << icp_trans << endl;
pcl::transformPointCloud(*source, *sac_icp, icp_trans);
pcl::io::savePLYFileASCII("sac_icp.ply", *sac_icp);
// -----------------点云可视化--------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final(new pcl::visualization::PCLVisualizer("配准结果"));
viewer_final->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color(target, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(sac_icp, 0, 0, 255);
viewer_final->addPointCloud<pcl::PointXYZ>(icp_result, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
while (!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
4.实验结果
