├── README.md
└── pcl
    ├── CMakeLists.txt
    ├── readme.txt
    └── supervoxel_clustering.cpp


/README.md:
--------------------------------------------------------------------------------
 1 | # pcl
 2 | pcl
 3 | 一，功能描述
 4 |   1，视差图点云创建，
 5 |   2，鼠标选取点云创建区域与待匹配物体，
 6 |   3，点云滤波，
 7 |   4，点云超体聚类，
 8 |   5，LCCP分割，
 9 |   6，点云降采样
10 |   7，ICP匹配
11 |   8，点云显示。
12 |   
13 | 二，功能解释
14 |   1，视差图点云创建
15 |      点云创建部分 使用四元数旋转参数 q(0.5,0,0,0) 与位移参数（0，0，0）使创建的点云为以相机坐标系为原点。
16 |      采用原图进行 1/4 采样的方式加速点云创建。
17 |   2，鼠标选取点云创建区域与待匹配物体
18 |      待创建区域的选取是在RGB图像上进行的，通过鼠标左键获取选取区域的对角坐标，存储在Pt1与pt2中，鼠标右键点选待匹配物体，坐标点存储在pt3中。
19 |   3，点云滤波
20 |      滤波主要采用了统计滤波器：pcl::StatisticalOutlierRemoval
21 |                 与半径滤波器：pcl::RadiusOutlierRemoval
22 |   4, 点云超体聚类
23 |      pcl::SupervoxelClustering<PointT> super(voxel_resolution, seed_resolution);
24 |      
25 |      https://www.cnblogs.com/ironstark/p/5013968.html
26 |      点云超体聚类对点云进行过分割。
27 |      在点云聚类中，没有使用颜色信息，所以将color_importance设置为0。
28 |      粒子距离、晶核距离 参数越小过分割越严重，对于最后的分割效果越精细。
29 |   5, LCCP分割
30 |      pcl::LCCPSegmentation<PointT> lccp;
31 |      
32 |      https://www.cnblogs.com/ironstark/p/5027269.html
33 |      LCCP分割利用物体的凹凸关系在超体聚类的基础上对聚类结果再聚类。
34 |      
35 |      通过超体聚类与LCCP分割之后会得到不同label标注的单个物体点云块。
36 |   6，点云降采样
37 |      在分割出来的单物体点云与点云库匹配之前，对点云库中的点云进行降采样，以减少计算量。
38 |      pcl::VoxelGrid<pcl::PointXYZ>  程序中采用0.004的网格进行采样。
39 |   7，ICP匹配
40 |      ICP匹配部分主要是将分割出来的单物体点云与点云库中物体进行匹配，以获得物体点云到云点云库物体匹配时的旋转矩阵。
41 |      pcl::IterativeClosestPoint<PointT,PointT> 
42 |      
43 |      最优匹配结果的选取是根据比较单个点云与点云库中物体匹配的得分进行选择，将匹配得分最好的一个输出保存。
44 |   8，点云显示
45 |      点云显示中采用pcl::visualization::PCLVisualizer类，支持给点云添加颜色、显示点云中的法矢、在窗口中自己画图案、自定义视角的位置 等功能。
46 |      
47 |      在本程序中将点云显示封装为函数，支持两个视窗，与单个视窗显示，并显示出点云坐标系。
48 |      pcl::visualization::PCLVisualizer  pcd_viewer(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)   为两个视窗显示函数。
49 |      
50 |      pcl::visualization::PCLVisualizer  pcd_viewer_one(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)  为单视窗显示函数。
51 |      
52 |      在显示函数中均支持单个视窗添加显示多个点云文件，显示匹配效果。
53 |      
54 |  
55 | 三，文件目录
56 |     --CMakeLists.txt
57 |     --supervoxel_clustering.cpp
58 |     --readme.txt
59 |     --obj   //待匹配点云库
60 |       --box1.pcd
61 |       ------
62 |       --box(n).pcd 
63 |     --data   
64 |     ----rgb //待分割RGB图
65 |     ----disp //待分割视差图
66 |     --build //程序编译目录
67 |     
68 | 四，编译运行
69 |     1，依赖项
70 |       Opencv2.4.9 or later
71 |         https://opencv.org/releases.html
72 |       PointCloud library 1.8
73 |         https://github.com/PointCloudLibrary/pcl
74 |      
75 |       eigen3.0
76 |         sudo apt-get install libeigen3.0-dev
77 |     2,程序编译
78 |       在工程目录下
79 |       mkdir build
80 |       cd build
81 |       cmake ..
82 |       make 
83 |      得到可执行文件 supervoxel_clustering
84 |      
85 |     3，程序运行
86 |      ./supervoxel_clustering disp rgb 17
87 |      运行示例 ，disp 为视差图子路径（data目录下） ，rgb 为RGB图子路径  ，17 图片序号
88 | 


--------------------------------------------------------------------------------
/pcl/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 2 | 
 3 | project(supervoxel_clustering)
 4 | set( CMAKE_BUILD_TYPE Release )
 5 | set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
 6 | 
 7 | find_package(PCL 1.8 REQUIRED)
 8 | # opencv 
 9 | find_package( OpenCV REQUIRED )
10 | include_directories( ${OpenCV_INCLUDE_DIRS} )
11 | # eigen 
12 | include_directories( "/usr/include/eigen3/" )
13 | 
14 | include_directories(${PCL_INCLUDE_DIRS})
15 | link_directories(${PCL_LIBRARY_DIRS})
16 | add_definitions(${PCL_DEFINITIONS})
17 | 
18 | add_executable (supervoxel_clustering supervoxel_clustering.cpp)
19 | target_link_libraries (supervoxel_clustering ${OpenCV_LIBS} ${PCL_LIBRARIES}) 
20 | 


--------------------------------------------------------------------------------
/pcl/readme.txt:
--------------------------------------------------------------------------------
 1 | 一，功能描述
 2 |   1，视差图点云创建，
 3 |   2，鼠标选取点云创建区域与待匹配物体，
 4 |   3，点云滤波，
 5 |   4，点云超体聚类，
 6 |   5，LCCP分割，
 7 |   6，点云降采样
 8 |   7，ICP匹配
 9 |   8，点云显示。
10 |   
11 | 二，功能解释
12 |   1，视差图点云创建
13 |      点云创建部分 使用四元数旋转参数 q(0.5,0,0,0) 与位移参数（0，0，0）使创建的点云为以相机坐标系为原点。
14 |      采用原图进行 1/4 采样的方式加速点云创建。
15 |   2，鼠标选取点云创建区域与待匹配物体
16 |      待创建区域的选取是在RGB图像上进行的，通过鼠标左键获取选取区域的对角坐标，存储在Pt1与pt2中，鼠标右键点选待匹配物体，坐标点存储在pt3中。
17 |   3，点云滤波
18 |      滤波主要采用了统计滤波器：pcl::StatisticalOutlierRemoval
19 |                 与半径滤波器：pcl::RadiusOutlierRemoval
20 |   4, 点云超体聚类
21 |      pcl::SupervoxelClustering<PointT> super(voxel_resolution, seed_resolution);
22 |      
23 |      https://www.cnblogs.com/ironstark/p/5013968.html
24 |      点云超体聚类对点云进行过分割。
25 |      在点云聚类中，没有使用颜色信息，所以将color_importance设置为0。
26 |      粒子距离、晶核距离 参数越小过分割越严重，对于最后的分割效果越精细。
27 |   5, LCCP分割
28 |      pcl::LCCPSegmentation<PointT> lccp;
29 |      
30 |      https://www.cnblogs.com/ironstark/p/5027269.html
31 |      LCCP分割利用物体的凹凸关系在超体聚类的基础上对聚类结果再聚类。
32 |      
33 |      通过超体聚类与LCCP分割之后会得到不同label标注的单个物体点云块。
34 |   6，点云降采样
35 |      在分割出来的单物体点云与点云库匹配之前，对点云库中的点云进行降采样，以减少计算量。
36 |      pcl::VoxelGrid<pcl::PointXYZ>  程序中采用0.004的网格进行采样。
37 |   7，ICP匹配
38 |      ICP匹配部分主要是将分割出来的单物体点云与点云库中物体进行匹配，以获得物体点云到云点云库物体匹配时的旋转矩阵。
39 |      pcl::IterativeClosestPoint<PointT,PointT> 
40 |      
41 |      最优匹配结果的选取是根据比较单个点云与点云库中物体匹配的得分进行选择，将匹配得分最好的一个输出保存。
42 |   8，点云显示
43 |      点云显示中采用pcl::visualization::PCLVisualizer类，支持给点云添加颜色、显示点云中的法矢、在窗口中自己画图案、自定义视角的位置 等功能。
44 |      
45 |      在本程序中将点云显示封装为函数，支持两个视窗，与单个视窗显示，并显示出点云坐标系。
46 |      pcl::visualization::PCLVisualizer  pcd_viewer(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)   为两个视窗显示函数。
47 |      
48 |      pcl::visualization::PCLVisualizer  pcd_viewer_one(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)  为单视窗显示函数。
49 |      
50 |      在显示函数中均支持单个视窗添加显示多个点云文件，显示匹配效果。
51 |      
52 |  
53 | 三，文件目录
54 |     --CMakeLists.txt
55 |     --supervoxel_clustering.cpp
56 |     --readme.txt
57 |     --obj   //待匹配点云库
58 |       --box1.pcd
59 |       ------
60 |       --box(n).pcd 
61 |     --data   
62 |     ----rgb //待分割RGB图
63 |     ----disp //待分割视差图
64 |     --build //程序编译目录
65 |     
66 | 四，编译运行
67 |     1，依赖项
68 |       Opencv2.4.9 or later
69 |         https://opencv.org/releases.html
70 |       PointCloud library 1.8
71 |         https://github.com/PointCloudLibrary/pcl
72 |      
73 |       eigen3.0
74 |         sudo apt-get install libeigen3.0-dev
75 |     2,程序编译
76 |       在工程目录下
77 |       mkdir build
78 |       cd build
79 |       cmake ..
80 |       make 
81 |      得到可执行文件 supervoxel_clustering
82 |      
83 |     3，程序运行
84 |      ./supervoxel_clustering disp rgb 17
85 |      运行示例 ，disp 为视差图子路径（data目录下） ，rgb 为RGB图子路径  ，17 图片序号
86 | 
87 |      
88 |      
89 |      
90 |      
91 |      
92 |      
93 |      
94 |      
95 |      
96 |      
97 |      
98 | 


--------------------------------------------------------------------------------
/pcl/supervoxel_clustering.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <fstream>
  3 | using namespace std;
  4 | #include <opencv2/core/core.hpp>
  5 | #include <opencv2/highgui/highgui.hpp>
  6 | #include <Eigen/Geometry> 
  7 | #include <boost/format.hpp>  // for formating strings
  8 | #include <pcl/point_types.h> 
  9 | #include <pcl/io/pcd_io.h> 
 10 | #include <pcl/visualization/pcl_visualizer.h>
 11 | #include <stdlib.h>  
 12 | #include <cmath>  
 13 | #include <boost/thread/thread.hpp>
 14 | #include <limits.h>  
 15 | #include <boost/format.hpp>  
 16 | #include <pcl/filters/voxel_grid.h>
 17 | #include <pcl/console/parse.h>  
 18 | #include <pcl/io/pcd_io.h>  
 19 | #include <pcl/visualization/pcl_visualizer.h>  
 20 | #include <pcl/visualization/cloud_viewer.h>
 21 | #include <pcl/visualization/point_cloud_color_handlers.h>  
 22 | #include <pcl/filters/passthrough.h>   
 23 | #include <pcl/segmentation/supervoxel_clustering.h>
 24 | #include <vtkPolyLine.h>   
 25 | #include <pcl/segmentation/lccp_segmentation.h>  
 26 | #include <pcl/registration/icp.h>
 27 | #include <pcl/filters/statistical_outlier_removal.h>
 28 | #include<pcl/filters/radius_outlier_removal.h>  
 29 | #include <pcl/console/time.h>   // TicToc
 30 | using namespace cv;
 31 | 
 32 | // 定义点云使用的格式：这里用的是XYZ
 33 | typedef pcl::PointXYZ PointT; 
 34 | typedef pcl::PointCloud<PointT> PointCloud; 
 35 | 
 36 | //×××××××××××××××××××××××点云区域选择有关×××××××××××××××××××××//
 37 | Point pt1 = Point(0, 0);
 38 | Point pt2 = Point(0, 0);
 39 | Point pt3 = Point(0, 0);
 40 | bool is_selecting = false;
 41 | bool object_seted = false;
 42 | void cvMouseCallback(int mouseEvent, int x, int y, int flags, void* param) // 鼠标选取区域，左键区域，右键物体 。其中左键区域非必须。
 43 | {
 44 |   switch (mouseEvent)
 45 |   {
 46 |     case CV_EVENT_LBUTTONDOWN:
 47 |       pt1 = Point(x, y);
 48 |       pt2 = Point(x, y);
 49 |       cout<<"*******PO1********"<<endl;
 50 |       is_selecting = true;
 51 |       break;
 52 |     case CV_EVENT_MOUSEMOVE:
 53 |       if (is_selecting)
 54 |          pt2 = Point(x, y);
 55 |          //cout<<"*******MOVE********"<<endl;
 56 |       break;
 57 |     case CV_EVENT_LBUTTONUP:
 58 |       pt2 = Point(x, y);
 59 |       is_selecting = false;
 60 |       object_seted = true;
 61 |       cout<<"*******PO2********"<<endl;
 62 |       break;
 63 |     case CV_EVENT_RBUTTONDOWN:
 64 |       pt3 = Point(x, y);
 65 |       object_seted = false;  
 66 |   }
 67 | }
 68 | //×××××××××××××××××××××××点云区域选择有关×××××××××××××××××××××//
 69 | 
 70 |  pcl::visualization::PCLVisualizer pcd_viewer(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best);
 71 |  pcl::visualization::PCLVisualizer pcd_viewer_one(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best);
 72 | void showpcd(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
 73 | int main(int argc, char ** argv)  
 74 | {   
 75 |     pcl::console::TicToc time; //运行计时
 76 | 
 77 |   //×××××××××××××××××××××××点云区域选择×××××××××××××××××××××//
 78 |     Mat rgb_mat ,rct_mat ,depth ;
 79 | 
 80 |     std::stringstream str_disp;
 81 |       str_disp << "../data/"<<argv[1]<<"/disp_" <<argv[3]<< ".png"; 
 82 |     std::stringstream str_rgb;
 83 |       str_rgb << "../data/"<<argv[2]<<"/rgb_"<<argv[3]<<".png"; 
 84 |     
 85 |     rgb_mat=cv::imread(str_rgb.str());
 86 |     namedWindow("rgb_mat", 0);
 87 |     resizeWindow("rgb_mat",rgb_mat.cols/2,rgb_mat.rows/2);
 88 |     imshow("rgb_mat",rgb_mat);
 89 | 
 90 |     cvSetMouseCallback("rgb_mat", cvMouseCallback);
 91 |     //while(! pt3.x);
 92 |     pt3.x =500, pt3.y = 200; //因新电脑问题，waitKey(n)没有效果，只能预给定值测试。老电脑运行正常。
 93 |     cout<<"P1.X= "<<pt1.x<<"   P1.Y="<<pt1.y<<endl;
 94 |     cout<<"P2.X= "<<pt2.x<<"   P2.Y="<<pt2.y<<endl;
 95 |     cout<<"P3.X= "<<pt3.x<<"   P3.Y="<<pt3.y<<endl;
 96 | 
 97 |     if(pt1.x)
 98 |     {
 99 |       rct_mat=rgb_mat(Rect(pt1.x,pt1.y,abs(pt2.x-pt1.x),abs(pt2.y-pt1.y)));
100 |       imshow("rct_mat",rct_mat);
101 |      
102 |       Mat disp = cv::imread(str_disp.str(), -1 );
103 |       depth = disp(Rect(pt1.x,pt1.y,abs(pt2.x-pt1.x),abs(pt2.y-pt1.y))); // 使用-1读取原始图像       
104 |     }
105 |     else
106 |     {
107 |       depth = cv::imread(str_disp.str(), -1 );
108 |     }
109 |   //×××××××××××××××××××××××点云区域选择结束×××××××××××××××××××××//
110 | 
111 |   //×××××××××××××××××××××××点云创建部分×××××××××××××××××××××//
112 |     time.tic();
113 |     PointT object_p ; //选择物体点
114 | 
115 |     Eigen::Quaterniond q( 0.5, 0, 0, 0 );
116 |     Eigen::Isometry3d Tr(q);
117 |     Tr.pretranslate( Eigen::Vector3d( 0, 0, 0));
118 |     // 相机内参 
119 |     double cx = 193.279*2;
120 |     double cy =  297.746*2;
121 |     double fx = 599.898*2;
122 |     double fy = 599.898*2;
123 |     double depthScale = 1000.0;
124 |  
125 |     // 新建一个点云
126 |     PointCloud::Ptr pointCloud( new PointCloud ); 
127 | 
128 |     for ( int v=0; v<depth.rows; v+=2 )
129 |        for ( int u=0; u<depth.cols; u+=2 )
130 |          {
131 |             unsigned short d = depth.ptr<unsigned short> ( v )[u]; // 深度值
132 |             if ( d==0 || d<8000 ) continue; // 为0表示没有测量到
133 |             Eigen::Vector3d point; 
134 |             double dep=(double)(71832*255)/d;
135 |             point[2] = double(dep)/depthScale; 
136 |             point[0] = (u-cx)*point[2]/fx;
137 |             point[1] = (v-cy)*point[2]/fy; 
138 |             Eigen::Vector3d pointWorld = Tr*point;
139 |                 
140 |             PointT p ;
141 |             p.x = pointWorld[0];
142 |             p.y = pointWorld[1];
143 |             p.z = pointWorld[2];
144 |             pointCloud->points.push_back( p );
145 |             if(((pt3.x-pt1.x)==u) && ((pt3.y-pt1.y)==v)) //选择物体点判断
146 |             { object_p=p; }
147 |           }
148 |     cout<<"object_p.x= "<<object_p.x<<"  object_p.y= "<<object_p.y<<"  object_p.z= "<<object_p.z<<endl;
149 |     std::cout <<  "Create cloud = " << time.toc () << " ms" << std::endl;
150 | 
151 |     pointCloud->is_dense = false;
152 |     cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
153 |     pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
154 |   //×××××××××××××××××××××××点云创建部分结束×××××××××××××××××××××//
155 | 
156 |   //×××××××××××××××××××××××点云滤波×××××××××××××××××××××//
157 |     time.tic();
158 |     PointCloud::Ptr pointCloud_filtered( new PointCloud );
159 |       // Create the filtering object
160 |     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //统计滤波器
161 |     sor.setInputCloud (pointCloud);
162 |     sor.setMeanK (50);              //设置在进行统计时考虑查询点临近点数
163 |     sor.setStddevMulThresh (1.0);   //设置判断是否为离群点的阀值
164 |     sor.filter (*pointCloud_filtered);
165 | 
166 | //    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem; //半径滤波器
167 | //    outrem.setInputCloud(pointCloud);
168 | //    outrem.setRadiusSearch(0.8);  
169 | //    outrem.setMinNeighborsInRadius (2);
170 | //    // apply filter
171 | //    outrem.filter (*pointCloud_filtered);
172 | 
173 | 
174 | //    pointCloud_filtered->is_dense = false;
175 | //    cout<<"filtered 点云共有"<<pointCloud_filtered->size()<<"个点."<<endl;
176 | //    std::stringstream str_fiter_save;
177 | //    str_fiter_save <<"filtered_" <<argv[3]<< ".pcd";
178 | //    pcl::io::savePCDFileBinary(str_fiter_save.str(), *pointCloud_filtered );
179 |     std::cout <<  "filter=  " << time.toc () << " ms" << std::endl;
180 |   //×××××××××××××××××××××××点云滤波结束×××××××××××××××××××××//
181 |   
182 |   //×××××××××××××××××××××××超体聚类×××××××××××××××××××××//
183 |     time.tic();
184 |     typedef pcl::LCCPSegmentation<PointT>::SupervoxelAdjacencyList SuperVoxelAdjacencyList; 
185 |     //超体聚类 参数依次是粒子距离、晶核距离、颜色容差、  
186 |     float voxel_resolution = 0.007f;   //与seed_resolution一般为2倍关系，参数越小过分割越严重，物体分的越细
187 |     float seed_resolution = 0.014f;  
188 |     float color_importance = 0.0f;  
189 |     float spatial_importance = 1.0f;  
190 |     float normal_importance = 1.0f;  
191 |     bool use_single_cam_transform = false;  
192 |     bool use_supervoxel_refinement = false;  
193 |     unsigned int k_factor = 0;    
194 |   
195 |     pcl::SupervoxelClustering<PointT> super(voxel_resolution, seed_resolution);  
196 |     super.setUseSingleCameraTransform(use_single_cam_transform);  
197 |     super.setInputCloud(pointCloud_filtered);
198 |     super.setColorImportance(color_importance);  
199 |     super.setSpatialImportance(spatial_importance);    
200 |     super.setNormalImportance(normal_importance);  
201 |     std::map<uint32_t, pcl::Supervoxel<PointT>::Ptr> supervoxel_clusters;   
202 |     super.extract(supervoxel_clusters);  
203 | 
204 |     std::multimap<uint32_t, uint32_t> supervoxel_adjacency;  
205 |     super.getSupervoxelAdjacency(supervoxel_adjacency);  
206 |     pcl::PointCloud<pcl::PointNormal>::Ptr sv_centroid_normal_cloud =pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud(supervoxel_clusters);  
207 |     std::cout <<  "Supervoxel in = " << time.toc () << " ms" << std::endl;
208 |   
209 | //  sv_centroid_normal_cloud ->height = 1;
210 | //  sv_centroid_normal_cloud ->width = sv_centroid_normal_cloud ->points.size();
211 | //    cout<<"Super point cloud size = "<<sv_centroid_normal_cloud ->points.size()<<endl;
212 | //    sv_centroid_normal_cloud ->is_dense = false;
213 | //    pcl::io::savePCDFile( "Super.pcd", *sv_centroid_normal_cloud ); 
214 |   //×××××××××××××××××××××××超体聚类结束×××××××××××××××××××××//
215 |   
216 |   //×××××××××××××××××××××××LCCP分割×××××××××××××××××××××//
217 |     time.tic ();
218 |     float concavity_tolerance_threshold = 9;  
219 |     float smoothness_threshold = 0.1;  
220 |     uint32_t min_segment_size = 0;  
221 |     bool use_extended_convexity = true;  
222 |     bool use_sanity_criterion = true;  
223 |   // PCL_INFO("Starting Segmentation\n");  
224 |     time.tic();
225 |     pcl::LCCPSegmentation<PointT> lccp;  //生成LCCP分割器
226 |   //CC效验beta值
227 |     lccp.setConcavityToleranceThreshold(concavity_tolerance_threshold);  
228 |     lccp.setSmoothnessCheck(true, voxel_resolution, seed_resolution, smoothness_threshold); 
229 |   //CC效验的k邻点 
230 |     lccp.setKFactor(k_factor);  
231 |   //输入超体聚类结果
232 |     lccp.setInputSupervoxels(supervoxel_clusters, supervoxel_adjacency); 
233 |   //SC效验
234 |     lccp.setSanityCheck (1); 
235 |   //最小分割尺寸
236 |     lccp.setMinSegmentSize(min_segment_size);  
237 |     lccp.segment();  
238 |  
239 |     pcl::PointCloud<pcl::PointXYZL>::Ptr sv_labeled_cloud = super.getLabeledCloud();  
240 |     pcl::PointCloud<pcl::PointXYZL>::Ptr lccp_labeled_cloud = sv_labeled_cloud->makeShared();  
241 |     lccp.relabelCloud(*lccp_labeled_cloud);  
242 | //   SuperVoxelAdjacencyList sv_adjacency_list;  
243 | //   lccp.getSVAdjacencyList(sv_adjacency_list);
244 | 
245 |     std::cout <<  "LCCP in=  " << time.toc () << " ms" << std::endl;
246 |     
247 |     cout<<"LCCP point cloud size = "<<lccp_labeled_cloud ->points.size()<<endl;
248 |     lccp_labeled_cloud ->is_dense = false;
249 |     pcl::io::savePCDFile( "LCCP.pcd", *lccp_labeled_cloud ); 
250 |   //×××××××××××××××××××××××LCCP分割结束×××××××××××××××××××××//
251 |   
252 |    //××××××××××××××××××××××得到点选物体×××××××××××××××××××××//
253 |     time.tic ();
254 |     int object_label=1000;
255 |     int label_max2 = 0,pcdn=0;  
256 |     for (int i = 0; i< lccp_labeled_cloud->size(); i++) 
257 |     {  
258 |       float det= 0;
259 |       if (lccp_labeled_cloud->points[i].label>label_max2)  
260 | 	  label_max2 = lccp_labeled_cloud->points[i].label;  
261 |       det =abs(object_p.x - lccp_labeled_cloud->points[i].x )+abs(object_p.y-lccp_labeled_cloud->points[i].y)+abs(object_p.z-lccp_labeled_cloud->points[i].z); //通过判断选取点误差确定选取物体的点云label.
262 |       if(det<0.001)
263 |       {
264 |         object_label=lccp_labeled_cloud->points[i].label;
265 |       }
266 |     }
267 |     cout<<"object_label= "<<object_label<<endl;
268 |     
269 |     PointCloud::Ptr pointCloud_seg( new PointCloud );
270 |     PointT object_label_p ;
271 |     int object_cot=0;
272 |     for (int j = 0; j < lccp_labeled_cloud->size(); j++) 
273 |     {  
274 |       if (lccp_labeled_cloud->points[j].label == object_label) 
275 |       {  
276 | 	    object_label_p.x = lccp_labeled_cloud->points[j].x;  
277 | 	    object_label_p.y = lccp_labeled_cloud->points[j].y;  
278 | 	    object_label_p.z = lccp_labeled_cloud->points[j].z;  
279 | 	    pointCloud_seg->points.push_back(object_label_p );
280 | 	    object_cot++;
281 | 	  }  
282 |     }
283 |     std::cout <<  "点选物体 in=  " << time.toc () << " ms" << std::endl;
284 |     cout<<"点选 point cloud size = "<<pointCloud_seg ->points.size()<<endl;
285 |     pointCloud_seg ->height = 1;
286 |     pointCloud_seg ->width = pointCloud_seg ->points.size();
287 |     pointCloud_seg->is_dense = false;
288 |     pcl::io::savePCDFile( "pointCloud_seg.pcd", *pointCloud_seg ); 
289 |    //××××××××××××××××××××××得到点选物体结束×××××××××××××××××××××//
290 |    //××××××××××××××××××××××ICP匹配×××××××××××××××××××××//
291 |     pcl::PCDReader reader;
292 |     PointCloud::Ptr tar_cloud (new PointCloud);
293 |     PointCloud::Ptr tar_filtered (new PointCloud);
294 |     PointCloud::Ptr cloud_tr (new PointCloud);    
295 |     PointCloud::Ptr Final_best(new PointCloud);  
296 |     
297 |     char obj_name[][20] = {"../obj/box1.pcd", "../obj/box2.pcd", "../obj/box3.pcd", "../obj/box4.pcd"};
298 |     double score_best = 10000;
299 |     int num_best = 0;
300 |     
301 |     for(int i=0; i<=3; i++)
302 |     {
303 |       time.tic ();
304 |       pcl::VoxelGrid<pcl::PointXYZ> sor;  //目标点云降采样。
305 | 	  reader.read(obj_name[i], *tar_cloud);        // 目标点云
306 |    	  sor.setInputCloud(tar_cloud);
307 |    	  sor.setLeafSize(0.004f, 0.004f, 0.004f);
308 |    	  sor.filter(*tar_filtered);
309 |    	  
310 |       pcl::IterativeClosestPoint<PointT,PointT> icp; //ICP点云匹配
311 |     //icp.setMaximumIterations(iterations);
312 |       icp.setInputTarget (tar_filtered);
313 |       icp.setInputSource (pointCloud_seg);
314 |       icp.setMaxCorrespondenceDistance(50);          // 设置对应点对之间的最大距离（此值对配准结果影响较大）
315 |       icp.setTransformationEpsilon(1e-10);           // 设置两次变化矩阵之间的差值（一般设置为1e-10即可）
316 |       icp.setEuclideanFitnessEpsilon(0.001);         // 设置收敛条件是均方误差和小于阈值，停止迭代；
317 |       icp.setMaximumIterations(500);                  // 最大迭代次数
318 |       
319 |       PointCloud::Ptr Final(new PointCloud);
320 |       icp.align(*Final);
321 |       //icp.align(*pointCloud_seg);
322 |      if (icp.hasConverged ())
323 |      {
324 |         if (icp.getFitnessScore() < score_best)
325 |         {
326 |           score_best = icp.getFitnessScore();
327 |           num_best = i+1;
328 |          // Final_best = Final;
329 |           *Final_best = *Final;
330 |         }
331 |        std::cout << "----------------------------------------------------------"<< std::endl;
332 |        std::cout << "has converged: " << icp.hasConverged() <<std::endl;
333 |        std::cout << "score: " <<icp.getFitnessScore() << std::endl; 
334 |        std::cout << icp.getFinalTransformation() << std::endl;  // 输出 RT 矩阵     
335 | 	  }	
336 | 	  std::cout <<  "ICP in=  " << time.toc () << " ms" << std::endl;		
337 |       
338 |      }
339 |      std::cout <<  "\nBest score: " << score_best << "\nObject: obj_" << num_best << ".pcd\n";
340 |      //*************************显示有关×××××××××××××××××××//
341 |      reader.read(obj_name[num_best-1], *tar_cloud); 
342 |      pcl::visualization::PCLVisualizer viewer;
343 |      viewer = pcd_viewer_one(tar_cloud,Final_best); //显示点云
344 |      while (!viewer.wasStopped ())
345 |      {
346 |       // cout<<"***************************"<<endl;
347 |        //viewer.spinOnce ();  //不加这一句显示出来的点云不能旋转
348 |      }
349 |      //*************************显示有关×××××××××××××××××××//
350 | //     std::stringstream str_final;
351 | //     str_final <<"final_" <<argv[3]<< ".pcd";
352 | //     pcl::io::savePCDFileBinary(str_final.str(), *Final_best ); 
353 |  
354 |     //××××××××××××××××××××××ICP匹配结束×××××××××××××××××××××//
355 |    
356 |     return 0;  
357 | } 
358 | 
359 | 
360 | void showpcd(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
361 | {
362 |   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
363 |   viewer.showCloud (cloud);
364 | }
365 | /////**********************************点云显示函数，两个视图的×××××××××××××××××××//
366 |  pcl::visualization::PCLVisualizer  pcd_viewer(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)
367 | {
368 |       pcl::visualization::PCLVisualizer viewer ("ICP demo");
369 |     // Create two verticaly separated viewports
370 |       int v1 (0);
371 |       int v2 (1);
372 |       viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
373 |       viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
374 | 
375 |     // The color we will be using
376 |       float bckgr_gray_level = 0.0;  // Black
377 |       float txt_gray_lvl = 1.0 - bckgr_gray_level;
378 |       
379 |     // Set background color
380 |       viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
381 |       viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
382 |       viewer.addCoordinateSystem (1.0); //添加坐标系
383 |     // Set camera position and orientation
384 |       viewer.setCameraPosition(-0.5, 0.5,-2, 0, -1, 0, 0); //前三个为坐标轴原点位置XYZ，后三个为视角，最后一个默认0
385 |    
386 |       // Original point cloud is white
387 |       pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (tar_cloud, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl); //颜色配置
388 |       viewer.addPointCloud (tar_cloud, cloud_in_color_h, "cloud_in_v1", v1);//左右视图显示库中点云
389 |       viewer.addPointCloud (tar_cloud, cloud_in_color_h, "cloud_in_v2", v2);
390 |       
391 |         // ICP aligned point cloud is red
392 |       pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (Final_best, 180, 20, 20);
393 |       viewer.addPointCloud (Final_best, cloud_icp_color_h, "cloud_icp_v2", v2); //右视图添加ICP旋转后的点云
394 |       viewer.setSize (1280, 1024);  // Visualiser window size
395 |       
396 |       return (viewer);
397 | }
398 | /////**********************************点云显示函数，一个视图的×××××××××××××××××××//
399 |  pcl::visualization::PCLVisualizer  pcd_viewer_one(pcl::PointCloud<pcl::PointXYZ>::Ptr tar_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr Final_best)
400 | {
401 |       pcl::visualization::PCLVisualizer viewer ("ICP demo");
402 |     // Create two verticaly separated viewports
403 | 
404 |     // The color we will be using
405 |       float bckgr_gray_level = 0.0;  // Black
406 |       float txt_gray_lvl = 1.0 - bckgr_gray_level;
407 |       
408 |     // Set background color
409 |       viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level);
410 |       
411 |     // Set camera position and orientation
412 |       viewer.setCameraPosition (-0.5, 0.5,-2, 0, -1, 0, 0); //前三个为坐标轴原点位置XYZ，后三个为视角，最后一个默认0
413 |    
414 |       // Original point cloud is white
415 |       pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (tar_cloud, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl); //颜色配置
416 |       viewer.addPointCloud (tar_cloud, cloud_in_color_h, "cloud_in_v1");//视图显示库中点云
417 |  
418 |         // ICP aligned point cloud is red
419 |       pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (Final_best, 180, 20, 20);
420 |       viewer.addPointCloud (Final_best, cloud_icp_color_h, "cloud_icp_v1"); //视图添加ICP旋转后的点云
421 |       viewer.addCoordinateSystem (1.0); //添加坐标系
422 |       viewer.setSize (1280, 1024);  // Visualiser window size
423 |       
424 |       return (viewer);
425 | }
426 | 
427 | 
428 | 
429 | 


--------------------------------------------------------------------------------