PFH+ICP算法点云配准研究，另附数据文件数据

FPFH+ICP算法点云配准研究，另附数据文件数据

注：有数据转换c++代码，可以把ply文件转换pcd格式文件
在网上搜了一些文章，感觉比较好的可供参考：
感谢博主的思路
基本思路为：
（1）从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d。
（2）在目标点云Q中查找与点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点。
（3）计算对应点之间刚体变换矩阵, 然后通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能。此处的距离误差和函数多使用Huber罚函数表示。

迭代最近点算法(Iterative Cloest Point, ICP)
ICP算法基于SVD，其大致思路如下:
(1) 将初始配准后的两片点云P′(经过坐标变换后的源点云)和Q，作为精配准的初始点集；
(2) 对源点云P’中的每一点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对；
(3) 初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对；
(4) 计算旋转矩阵R和平移向量T，使最小，即对应点集之间的均方误差最小；
(5) 设定某一阈值ε=dk-dk-1和最大迭代次数Nmax, 将上一步得到的刚体变换作用于源点云P′，得到新点云P”，计算P”和Q的距离误差,，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P”和Q，继续重复上述步骤，直至满足收敛条件。
ICP算法对参数敏感，在使用前要设置一下几个参数：
1.setMaximumIterations，最大迭代次数，icp是一个迭代的方法，最多迭代这些次；
2. setEuclideanFitnessEpsilon，设置收敛条件是均方误差和小于阈值，停止迭代；
3. setTransformtionEpsilon, 设置两次变化矩阵之间的差值（一般设置为1e-10即可）；
4. setMaxCorrespondenaceDistance,设置对应点对之间的最大距离（此值对配准结果影响较大）。
在两点云相差较大的情况下，ICP算法容易陷入局部最优解，从而无法得到较好的匹配结果，故需要先给定一个初始变换矩阵。在pcl库中的registration模块可实现ICP算法。

##代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

#include <iostream>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ pointT;
typedef pcl::PointCloud<pointT> PointCloud;

int
main(int argc, char **argv)
{
//加载点云文件
PointCloud::Ptr cloud_src_o(new PointCloud);//源点云
PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
if (pcl::io::loadPCDFile("bun000.pcd", *cloud_src_o) == -1)
{
PCL_ERROR("couldn'e file! ");
return(-1);
}

if (pcl::io::loadPCDFile("bun045.pcd", *cloud_tgt_o) == -1)
{
PCL_ERROR("couldn'e file! ");
return(-1);
}
clock_t start = clock();

//出去NAN点
std::vector<int> indices_src;
pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
std::cout << "remove *cloud_src_o nan !"<<std::endl;

//下采样
pcl::VoxelGrid<pointT> voxel_grid;
voxel_grid.setLeafSize(0.012, 0.012, 0.012);
voxel_grid.setInputCloud(cloud_src_o);
PointCloud::Ptr cloud_src(new PointCloud);
voxel_grid.filter(*cloud_src);
std::cout << "down size *cloud_src_o from " << cloud_src_o->points.size() << "to" << cloud_src->size() << endl;
pcl::io::savePCDFileASCII("bunny_src_down.pcd", *cloud_src);

//计算表面法线
pcl::NormalEstimation<pointT, pcl::Normal> ne_src;
ne_src.setInputCloud(cloud_src);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree<pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud<pcl::Normal>);
ne_src.setRadiusSearch(0.02);
ne_src.compute(*cloud_src_normals);

std::vector<int> indices_tgt;
pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);
std::cout << "remove *cloud_tgt_o nan! " << std::endl;

pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
voxel_grid_2.setLeafSize(0.012, 0.012,0.012);
voxel_grid_2.setInputCloud(cloud_tgt_o);
PointCloud::Ptr cloud_tgt(new pcl::PointCloud<pcl::PointXYZ>);
voxel_grid_2.filter(*cloud_tgt);
std::cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->points.size() << endl;
pcl::io::savePCDFileASCII("bunny_tgt_down.pcd", *cloud_tgt);

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
ne_tgt.setInputCloud(cloud_tgt);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree<pcl::PointXYZ>());
ne_tgt.setSearchMethod(tree_tgt);
pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud<pcl::Normal>);
ne_tgt.setRadiusSearch(0.02);
ne_tgt.compute(*cloud_tgt_normals);

//计算FPFH
pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_src;
fpfh_src.setInputCloud(cloud_src);
fpfh_src.setInputNormals(cloud_src_normals);
pcl::search::KdTree<pointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<pointT>);
fpfh_src.setSearchMethod(tree_src_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_src.setRadiusSearch(0.05);
fpfh_src.compute(*fpfhs_src);
std::cout << "compute *cloud_src fpfh" << endl;

pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
fpfh_tgt.setInputCloud(cloud_tgt);
fpfh_tgt.setInputNormals(cloud_tgt_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<pcl::PointXYZ>);
fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>);
fpfh_tgt.setRadiusSearch(0.05);
fpfh_tgt.compute(*fpfhs_tgt);
std::cout << "compute *cloud_tgt fpfh" << endl;

//SAC配准
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(cloud_src);
scia.setInputTarget(cloud_tgt);
scia.setSourceFeatures(fpfhs_src);
scia.setTargetFeatures(fpfhs_tgt);
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
std::cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
std::cout << sac_trans << endl;
pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);
clock_t sac_time = clock();

//icp配准
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_src);
icp.setInputTarget(cloud_tgt);
icp.setMaxCorrespondenceDistance(0.04);
//最大迭代次数
icp.setMaximumIterations(50);
//两次变化矩阵之间的差值
icp.setTransformationEpsilon(1e-10);
//均方误差
icp.setEuclideanFitnessEpsilon(0.2);
icp.align(*icp_result, sac_trans);
clock_t end = clock();
cout << "total time : " << (double)(end - start) / (double)CLOCKS_PER_SEC << "s" << endl;
//我把计算法线和点特征直方图的时间也算在SAC里面了
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;
std::cout << "ICP has converged:" << icp.hasConverged()
<< " score: " << icp.getFitnessScore() << std::endl;

Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
std::cout << "the icp.getfinaltransformation is " << icp_trans << endl;
//使用创建的变换对未过滤的点云进行变换
pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
pcl::io::savePCDFileASCII("bunny_transformed_sac_ndt.pcd", *icp_result);
return (0);
}

生成的图片：
点云配准完成后
配准前的点云
配准前图像
##数据来源一般在网上下载斯坦福大学的数据，但是其数据格式是ply格式，需要转换到pcd格式
斯坦福大学数据链接

转换的代码为

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/io/vtk_io.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

int main()
{
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
////pcl::io::loadPCDFile("model1.pcd",*cloud);

////ply文件显示
//pcl::PolygonMesh mesh;
//vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();

//pcl::io::loadPolygonFilePLY("raw.ply", mesh);
//// ply另存vtk
////pcl::io::saveVTKFile("temp.vtk", mesh);
//pcl::io::mesh2vtk(mesh, polydata);

//pcl::io::vtkPolyDataToPointCloud(polydata, *cloud);

////两种存贮方式 pcd另存pcd
//pcl::PCDWriter pcdwriter;
////pcdwriter.write<pcl::PointXYZRGBA>("save_ply2vtk2pcd.pcd", *cloud);
//pcl::io::savePCDFileASCII("raw1.pcd", *cloud);

//*VCGLIB生成的ply
/*pcl::PCLPointCloud2 clod;
pcl::io::loadPLYFile("raw.ply", clod);
pcl::io::savePCDFile("raw11.pcd", clod);*/

pcl::PCLPointCloud2 clod;
pcl::PLYReader reader;
reader.read("bun045.ply", clod);
pcl::PCDWriter writer;
writer.writeASCII("bun045.pcd", clod);

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
//对于ply文件中格式为RGBA格式，则上句改写为
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
pcl::io::loadPCDFile("raw11.pcd", *cloud);

/*
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewe(new pcl::visualization::PCLVisualizer("ss"));
//viewe->initCameraParameters();
viewe->setBackgroundColor(0, 0, 0);
//viewe->addCoordinateSystem(1.0f);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> color(cloud);
//对于ply文件中格式为RGBA格式，则上句改写为
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color(cloud);
viewe->addPointCloud<pcl::PointXYZRGB>(cloud, color, "cloud");
//对于ply文件中格式为RGBA格式，则上句改写为
//viewe->addPointCloud<pcl::PointXYZRGBA>(cloud, color, "cloud");

//viewe->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 2, "cloud");
while (!viewe->wasStopped()) {
viewe->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
*/
return 0;

}

上面的代码可以转换成无RGB信息的三维点云。也可以在我的百度网盘里直接下载，仅供参考：
百度网盘链接
提取吗：47oq