├── CMakeLists.txt ├── README.md ├── include ├── DefectDetect.h ├── Utility.h ├── config.h ├── dbscan.h └── pointcloud_helper.h ├── parameters └── parameter.yml └── src ├── DefectDetect.cpp ├── DefectDetect_demo.cpp ├── dbscan.cpp ├── demo ├── CMakeLists.txt ├── iterative_closest_point.cpp └── normal_distributions_transform.cpp ├── deprecated └── spacialnet.cpp ├── doc └── pcl help functions.cpp ├── fit_plane_from_file.cpp ├── libs ├── CMakeLists.txt ├── Utility.cc ├── config.cc └── pointcloud_helper.cc ├── normalEstimate_demo.cpp ├── normalEstimate_pcl_website.cpp ├── python cv2.txt~ └── tools ├── CMakeLists.txt ├── pcd2ply.cpp └── ply2pcd.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(3dDefectDetection) 3 | 4 | #SET(CMAKE_BUILD_TYPE "Debug") 5 | SET(CMAKE_BUILD_TYPE "Release") 6 | 7 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) 8 | 9 | ## Compile as C++11, supported in ROS Kinetic and newer 10 | add_compile_options(-std=c++11) 11 | 12 | find_package(PCL REQUIRED) 13 | include_directories(${PCL_INCLUDE_DIRS}) 14 | link_directories(${PCL_LIBRARY_DIRS}) 15 | add_definitions(${PCL_DEFINITIONS}) 16 | 17 | find_package(OpenCV REQUIRED) 18 | include_directories(${OpenCV_INCLUDE_DIRS}) 19 | link_directories(${OpenCV_LIBRARY_DIRS}) 20 | add_definitions(${OpenCV_DEFINITIONS}) 21 | 22 | include_directories(include) 23 | 24 | #needs to be put after find_package()? yes. they depend on PCL OpenCV 25 | #add_library() can be linked between sub_dirs? seems yes. to be confirmed. 26 | add_subdirectory(src/tools) 27 | add_subdirectory(src/libs) 28 | add_subdirectory(src/demo) 29 | 30 | 31 | ##################################### build ##################################################### 32 | add_executable(normalEstimate_demo src/normalEstimate_demo.cpp src/dbscan.cpp) 33 | target_link_libraries (normalEstimate_demo ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} config pointcloud_helper) 34 | 35 | add_executable(defectdetect_demo src/DefectDetect_demo.cpp src/dbscan.cpp src/DefectDetect.cpp) 36 | target_link_libraries (defectdetect_demo ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} config pointcloud_helper) 37 | 38 | add_executable(normalEstimate_pcl_website src/normalEstimate_pcl_website.cpp ) 39 | target_link_libraries (normalEstimate_pcl_website ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 40 | 41 | add_executable(fit_plane_from_file src/fit_plane_from_file.cpp ) 42 | target_link_libraries (fit_plane_from_file ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} config pointcloud_helper) 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ### overview 2 | This is a package to load point cloud data from file and detect defect. 3 | It's used to detect defect on plane. 4 | It's simplely calculate the plane equation and transform the plane parallel to XoY plane. 5 | Then calculate normal and curve. outliers is seen as results. 6 | 7 | Note: 8 | 1. Input file can be .pcd/.ply/.PCD/.PLY format. 9 | 2. It will transform data to mm unit, like all data is 50.86 level instead of 0.05086. 10 | 3. There are also many useful tools like ply2pcd and pcd2ply, used to converte format between point cloud files. 11 | 4. It's a good example of cmake project. 12 | 13 | #### to run the code 14 | ``` 15 | mkdir build 16 | cd build 17 | cmake .. 18 | make -j8 19 | sudo make install 20 | make clean 21 | 22 | ./bin/normalEstimate ../data/xxx.pcd 23 | ``` -------------------------------------------------------------------------------- /include/DefectDetect.h: -------------------------------------------------------------------------------- 1 | #ifndef DEFECTDETECT_H 2 | #define DEFECTDETECT_H 3 | 4 | //C++ 5 | #include 6 | #include 7 | #include 8 | 9 | //boost 10 | #include 11 | 12 | //PCL 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | //OpenCV 25 | #include 26 | 27 | //Customed 28 | #include "config.h" 29 | #include "pointcloud_helper.h" 30 | #include "dbscan.h" 31 | 32 | using namespace std; 33 | using namespace cv; 34 | 35 | #ifndef PI 36 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 37 | #endif 38 | #ifndef pi 39 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 40 | #endif 41 | int process(pcl::PointCloud::Ptr origin_cloud); 42 | #endif -------------------------------------------------------------------------------- /include/Utility.h: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | #include 4 | 5 | //OpenCV 6 | #include "opencv2/imgproc/imgproc.hpp" 7 | 8 | using namespace std; 9 | using namespace cv; 10 | 11 | //functions 12 | Mat ransac(int num, std::vector samples, int max_iteration, double threshold_In, double& average); 13 | -------------------------------------------------------------------------------- /include/config.h: -------------------------------------------------------------------------------- 1 | #ifndef _CV_CONFIG_H_ 2 | #define _CV_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | class Config 7 | { 8 | private: 9 | static Config* config_; 10 | cv::FileStorage file_; 11 | 12 | Config(){}; //private constructor makes a singleton 13 | 14 | public: 15 | ~Config(); 16 | 17 | //set a new config file 18 | static void setParameterFile(const std::string& fileName); 19 | 20 | //access the parameter values 21 | template 22 | static T get(const std::string& key) 23 | { 24 | T res; 25 | Config::config_->file_[key] >> res; 26 | return res; 27 | } 28 | 29 | 30 | }; 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /include/dbscan.h: -------------------------------------------------------------------------------- 1 | #ifndef DBSCAN_H 2 | #define DBSCAN_H 3 | 4 | #include 5 | 6 | namespace dbscn 7 | { 8 | struct Point{ 9 | double x; 10 | double y; 11 | }; 12 | 13 | int dbscan(const std::vector &input, std::vector &labels, double eps, int min); 14 | } 15 | #endif /*DBSCAN_H*/ 16 | -------------------------------------------------------------------------------- /include/pointcloud_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef _ZIQI_POINTCLOUD_HELPER_ 2 | #define _ZIQI_POINTCLOUD_HELPER_ 3 | //C++ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //OpenCV 14 | #include 15 | 16 | //PCL 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | //Eigen 32 | #include 33 | 34 | //Customed 35 | #include "pointcloud_helper.h" 36 | 37 | #ifndef PI 38 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 39 | #endif 40 | #ifndef pi 41 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 42 | #endif 43 | 44 | using namespace cv; 45 | using namespace std; 46 | using namespace pcl; 47 | 48 | cv::Mat getR2registeZ(pcl::PointCloud::Ptr cloud); 49 | int loadPointCloudData(string inputFilename, pcl::PointCloud::Ptr cloud); 50 | Eigen::Matrix4f E2R(const double alpha, const double beta, const double gamma); 51 | void getCenter(pcl::PointCloud::Ptr& cloud, float &x, float &y, float &z); 52 | Eigen::Matrix4f planeAlign(const pcl::PointCloud::Ptr& cloud); 53 | pcl::Normal getLoopNormal( 54 | const pcl::PointCloud::Ptr& cloud, 55 | const pcl::PointCloud::Ptr& normal, 56 | pcl::PointXYZ& outer_max, 57 | pcl::PointXYZ& outer_min, 58 | pcl::PointXYZ& iner_max, 59 | pcl::PointXYZ& iner_min); 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /parameters/parameter.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | ########################################### pre process before fitting plane ########################################### 4 | #对点云进行平面拟合（可选先进行体素滤波和带通滤波以加快处理速度），并且将点云旋转到传感器轴与测量平面垂直 5 | VoxelGrid_filter_before_fitting_plane: 1 6 | VoxelGrid_filter_before_fitting_plane_x: 10 #mm 7 | VoxelGrid_filter_before_fitting_plane_y: 10 #mm 8 | VoxelGrid_filter_before_fitting_plane_z: 10 #mm 9 | 10 | #对点云进行平面拟合（可选先进行体素滤波和带通滤波以加快处理速度），并且将点云旋转到传感器轴与测量平面垂直 11 | #we use VoxelGrid filter in the code for now, but you can also use PassThrough filter 12 | PASS_filter_before_fitting_plane: 1 13 | PASS_filter_x_before_fitting_plane: "x" 14 | PASS_filter_y_before_fitting_plane: "y" 15 | PASS_filter_x_min_before_fitting_plane: -20 #mm 16 | PASS_filter_x_max_before_fitting_plane: 20 #mm 17 | PASS_filter_y_min_before_fitting_plane: -20 #mm 18 | PASS_filter_y_max_before_fitting_plane: 20 #mm 19 | 20 | 21 | 22 | ########################################### pre process before normal estimate ########################################### 23 | VoxelGrid_filter: 0 24 | VoxelGrid_x: 0.4 #mm 25 | VoxelGrid_y: 0.4 #mm 26 | VoxelGrid_z: 0.4 #mm 27 | 28 | #pass filter is used, so that we only detect defect within this area to speed up. 29 | PASS_filter: 1 30 | PASS_filter_x: "x" 31 | PASS_filter_y: "y" 32 | PASS_filter_x_min: -20 #mm 33 | PASS_filter_x_max: 20 #mm 34 | PASS_filter_y_min: -20 #mm 35 | PASS_filter_y_max: 20 #mm 36 | 37 | 38 | 39 | ################################################## Normal estimate ################################################## 40 | RadiusSearch: 1 #mm used for searching K neighbour to estimate normal 41 | show_normal: 0 42 | 43 | max_theta_to_ZAxis: 5 #deg 44 | 45 | curvature_expect_overshot_ratio: 4 46 | curvature_expect_down_ratio: 0.8 #easy to be effected by max values. 47 | expect_z_overshot: 0.5 48 | 49 | image_width: 1920 50 | image_height: 1080 51 | 52 | 53 | 54 | ################################################## DBSCN PARAM ################################################## 55 | exp: 1 #distanse to check neighbors 56 | MinPt: 15 #min point size to be a cluster, otherwise be lables to noise. 57 | 58 | 59 | 60 | ############################################# Defect Selection PARAM ############################################# 61 | MinPtDefect: 10 #min point size in a cluster to be a Defect, otherwise this cluster will be deleted. 62 | 63 | #offset to filter fake Defect caused by edge. 64 | PASS_filter_x_min_offset: -17 #mm 65 | PASS_filter_x_max_offset: 17 #mm 66 | PASS_filter_y_min_offset: -17 #mm 67 | PASS_filter_y_max_offset: 17 #mm 68 | 69 | 70 | normal_estimate_area_ratio: 3 71 | -------------------------------------------------------------------------------- /src/DefectDetect.cpp: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | #include 4 | #include 5 | 6 | //boost 7 | #include 8 | 9 | //PCL 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //OpenCV 22 | #include 23 | 24 | //Customed 25 | #include "config.h" 26 | #include "pointcloud_helper.h" 27 | #include "dbscan.h" 28 | #include "DefectDetect.h" 29 | 30 | 31 | using namespace std; 32 | using namespace cv; 33 | 34 | #ifndef PI 35 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 36 | #endif 37 | #ifndef pi 38 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 39 | #endif 40 | 41 | 42 | 43 | int process(pcl::PointCloud::Ptr origin_cloud) 44 | { 45 | // string paraFileName = "../parameters/parameter.yml"; 46 | // Config::setParameterFile(paraFileName); 47 | 48 | //创建点云对象 49 | // pcl::PointCloud::Ptr origin_cloud(new pcl::PointCloud); 50 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 51 | //创建法线的对象 52 | pcl::PointCloud::Ptr normals(new pcl::PointCloud); 53 | //记录点云对齐的缸体变换矩阵 54 | Eigen::Matrix4f tf; 55 | 56 | //加载点云数据，命令行参数为文件名，文件可以是.PCD/.pcd/.PLY/.ply四种文件之一，单位可以是mm也可以是m. 57 | // string inputFilename=argv[1]; 58 | // if(loadPointCloudData(inputFilename, origin_cloud) != 0) 59 | // exit(-1); 60 | pcl::copyPointCloud(*origin_cloud, *cloud); 61 | 62 | //计算点云分布范围，如果分布范围大于1，说明单位是mm，小于1，说明单位是m. 63 | //用于存放三个轴的最小值 64 | pcl::PointXYZ min; 65 | //用于存放三个轴的最大值 66 | pcl::PointXYZ max; 67 | pcl::getMinMax3D(*cloud,min,max); 68 | std::cout<<"min.x = "<::iterator pt = cloud->points.begin(); pt < cloud->points.end(); pt++) 83 | { 84 | pt->x =1000*pt->x; 85 | pt->y =1000*pt->y; 86 | pt->z =1000*pt->z; 87 | } 88 | max.x = 1000*max.x; 89 | min.x = 1000*min.x; 90 | max.y = 1000*max.y; 91 | min.y = 1000*min.y; 92 | max.z = 1000*max.z; 93 | min.z = 1000*min.z; 94 | std::cout<<"min.x = "<setBackgroundColor (0, 0, 0); 101 | viewer->addCoordinateSystem(0.3*(max.y-min.y)); 102 | pcl::visualization::PointCloudColorHandlerCustom single_color (cloud, 0, 255, 0); 103 | viewer->addPointCloud(cloud, single_color, "Original"); 104 | while(!viewer->wasStopped()) 105 | { 106 | viewer->spinOnce(100); 107 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 108 | } 109 | 110 | 111 | 112 | 113 | //对点云进行平面拟合（可选先进行提速滤波和带通滤波以加快处理速度），并且将点云旋转到传感器轴与测量平面垂直 114 | if (Config::get("VoxelGrid_filter_before_fitting_plane")) 115 | { 116 | std::cout<<"VoxelGrid_filter_before_fitting_plane: "<("VoxelGrid_filter_before_fitting_plane")<::Ptr cloud_filtered(new pcl::PointCloud); 118 | pcl::VoxelGrid vg; 119 | //创建滤波对象 120 | vg.setInputCloud (cloud); 121 | //设置需要过滤的点云给滤波对象 122 | vg.setLeafSize ( 123 | Config::get("VoxelGrid_filter_before_fitting_plane_x"), 124 | Config::get("VoxelGrid_filter_before_fitting_plane_y"), 125 | Config::get("VoxelGrid_filter_before_fitting_plane_z")); 126 | //设置滤波时创建的体素体积为1cm的立方体 127 | vg.filter (*cloud_filtered); 128 | //执行滤波 129 | 130 | cv::Mat cv_R = getR2registeZ(cloud_filtered); 131 | Eigen::Matrix4f tf; 132 | for (int i = 0; i < 3; ++i) 133 | { 134 | for (int j = 0; j < 3; ++j) 135 | { 136 | tf(i,j)=cv_R.at(i,j); 137 | } 138 | } 139 | for (int i = 0; i < 4; ++i) 140 | { 141 | tf(i,3) = 0; 142 | tf(3,i) = 0; 143 | } 144 | tf(3,3)=1; 145 | std::cout<<"tf Matrix4f:\n"<(i,j); 158 | } 159 | } 160 | for (int i = 0; i < 4; ++i) 161 | { 162 | tf(i,3) = 0; 163 | tf(3,i) = 0; 164 | } 165 | tf(3,3)=1; 166 | std::cout<<"tf Matrix4f:\n"< viewer2(new pcl::visualization::PCLVisualizer("Cloud Transformed")); 172 | viewer2->setBackgroundColor (0, 0, 0); 173 | viewer2->addCoordinateSystem(0.3*(max.y-min.y)); 174 | pcl::visualization::PointCloudColorHandlerCustom single_color2 (cloud, 0, 255, 0); 175 | viewer2->addPointCloud(cloud, single_color, "Transformed"); 176 | while(!viewer2->wasStopped()) 177 | { 178 | viewer2->spinOnce(100); 179 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 180 | } 181 | 182 | 183 | 184 | 185 | if (Config::get("VoxelGrid_filter") && !Config::get("PASS_filter")) 186 | { 187 | std::cout<<"VoxelGrid_filter: "<("VoxelGrid_filter")<::Ptr cloud_filtered(new pcl::PointCloud); 189 | pcl::VoxelGrid vg; 190 | //创建滤波对象 191 | vg.setInputCloud (cloud); 192 | //设置需要过滤的点云给滤波对象 193 | vg.setLeafSize (Config::get("VoxelGrid_x"), Config::get("VoxelGrid_y"), Config::get("VoxelGrid_z")); 194 | //设置滤波时创建的体素体积为1cm的立方体 195 | vg.filter (*cloud_filtered); 196 | //执行滤波 197 | 198 | 199 | 200 | //创建法线估计的对象 201 | pcl::NormalEstimation normalEstimation; 202 | normalEstimation.setInputCloud(cloud_filtered); 203 | //对于每一个点都用半径为3cm的近邻搜索方式 204 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 205 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 206 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 207 | normalEstimation.setSearchMethod(kdtree); 208 | //计算法线 209 | normalEstimation.compute(*normals); 210 | 211 | 212 | 213 | //可视化 214 | boost::shared_ptr viewer3(new pcl::visualization::PCLVisualizer("Cloud Normals")); 215 | viewer3->setBackgroundColor (0, 0, 0); 216 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 217 | // Coloring and visualizing target cloud (red). 218 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud_filtered, 255, 0, 0); 219 | viewer3->addPointCloud (cloud_filtered, target_color, "cloud_filtered"); 220 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_filtered"); 221 | viewer3->addPointCloudNormals(cloud_filtered, normals, 1, (0.1*(max.x-min.x)), "normals"); 222 | while(!viewer3->wasStopped()) 223 | { 224 | viewer3->spinOnce(100); 225 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 226 | } 227 | } 228 | else if (Config::get("PASS_filter") && !Config::get("VoxelGrid_filter")) 229 | { 230 | std::cout<<"PASS_filter: "<("PASS_filter")<::Ptr cloud_filtered(new pcl::PointCloud); 232 | pcl::PassThrough pass; 233 | pass.setInputCloud (cloud); 234 | pass.setFilterFieldName (Config::get("PASS_filter_x")); 235 | pass.setFilterLimits (Config::get("PASS_filter_x_min"), Config::get("PASS_filter_x_max")); 236 | std::cout<<"PASS_filter_x_min: "<("PASS_filter_x_min")<("PASS_filter_x_max")<("PASS_filter_y")); 241 | pass.setFilterLimits (Config::get("PASS_filter_y_min"), Config::get("PASS_filter_y_max")); 242 | std::cout<<"PASS_filter_y_min: "<("PASS_filter_y_min")<("PASS_filter_y_max")< normalEstimation; 250 | normalEstimation.setInputCloud(cloud_filtered); 251 | //对于每一个点都用半径为3cm的近邻搜索方式 252 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 253 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 254 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 255 | normalEstimation.setSearchMethod(kdtree); 256 | //计算法线 257 | normalEstimation.compute(*normals); 258 | 259 | 260 | 261 | 262 | /* Detect defect using normal_z*/ 263 | float max_theta_to_ZAxis = Config::get("max_theta_to_ZAxis"); 264 | std::cout<<"max_theta_to_ZAxis is: "< index; 267 | int tmp_index=0; 268 | 269 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 270 | { 271 | if (std::abs(pt->normal_z) < abs_z_expect) 272 | { 273 | index.push_back(tmp_index); 274 | std::cout<<"index: "<normal_x <<" normal_y: "<normal_y 275 | <<" normal_z: "<normal_z <<" abs_z_expect: "<::Ptr cloudCP(new pcl::PointCloud); 280 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP); 281 | 282 | 283 | 284 | 285 | /* Detect defect using curvature*/ 286 | tmp_index =0; 287 | index.clear(); 288 | float sum_curvature=0; 289 | float max_curvature=0; 290 | float avg_curvature=0; 291 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 292 | { 293 | sum_curvature =sum_curvature + pt->curvature; 294 | if (max_curvature < pt->curvature) 295 | { 296 | max_curvature = pt->curvature; 297 | } 298 | } 299 | avg_curvature = sum_curvature/normals->points.size(); 300 | // float curvature_expect = avg_curvature*Config::get("curvature_expect_overshot_ratio"); 301 | float curvature_expect = max_curvature-(max_curvature-avg_curvature)*Config::get("curvature_expect_down_ratio"); 302 | 303 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 304 | { 305 | if (std::abs(pt->curvature) > curvature_expect) 306 | { 307 | index.push_back(tmp_index); 308 | std::cout<<"index: "<curvature<<" curvature_expect: "<::Ptr cloudCP2(new pcl::PointCloud); 313 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP2); 314 | 315 | 316 | 317 | 318 | /* Detect defect using pointcloud_z*/ 319 | float sum_z=0; 320 | float max_z=0; 321 | float avg_z=0; 322 | for(pcl::PointCloud::iterator pt = cloud_filtered->points.begin(); pt < cloud_filtered->points.end(); pt++) 323 | { 324 | sum_z = sum_z+ pt->z; 325 | if (max_z < pt->z) 326 | { 327 | max_z = pt->z; 328 | } 329 | } 330 | avg_z = sum_z/cloud_filtered->points.size(); 331 | tmp_index =0; 332 | index.clear(); 333 | float expect_z_overshot = avg_z + Config::get("expect_z_overshot"); 334 | 335 | for(pcl::PointCloud::iterator pt = cloud_filtered->points.begin(); pt < cloud_filtered->points.end(); pt++) 336 | { 337 | if (std::abs(pt->z) > expect_z_overshot) 338 | { 339 | index.push_back(tmp_index); 340 | std::cout<<"index: "<z: "<< pt->z<<" expect_z_overshot: "<::Ptr cloudCP3(new pcl::PointCloud); 345 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP3); 346 | 347 | 348 | 349 | /*Cluster using DBSCN*/ 350 | std::vector points; 351 | dbscn::Point tmp_pt; 352 | for (size_t i = 0; i < cloudCP->points.size(); ++i) 353 | { 354 | tmp_pt.x=cloudCP->points[i].x; 355 | tmp_pt.y=cloudCP->points[i].y; 356 | points.push_back(tmp_pt); 357 | } 358 | vector labels; 359 | int num_cluster = dbscn::dbscan(points, labels, Config::get("exp"), Config::get("MinPt")); 360 | cout<<"cluster size is "< viewer3(new pcl::visualization::PCLVisualizer("Defect Dtect Results")); 371 | viewer3->setBackgroundColor (0, 0, 0); 372 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 373 | if (Config::get("show_normal")) 374 | { 375 | viewer3->addPointCloudNormals(cloud_filtered, normals, 1, (0.1*(max.x-min.x)), "normals"); 376 | } 377 | // pcl::visualization::PointCloudColorHandlerCustom cloud_color (cloud_filtered, 125, 125, 125); 378 | // viewer3->addPointCloud (cloud, cloud_color, "cloud"); 379 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud_filtered, 0, 255, 0); 380 | viewer3->addPointCloud (cloud_filtered, target_color, "cloud_filtered"); 381 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_filtered"); 382 | 383 | for (int i = 1; i <= num_cluster; i++) 384 | { 385 | stringstream ss; 386 | ss<::Ptr cloudCPn(new pcl::PointCloud); 396 | pcl::copyPointCloud(*cloudCP, index, *cloudCPn); 397 | std::string cloud_id="cloudCP_"+ss.str(); 398 | std::cout<<"\ncloud_id: "<points.size() <<", MinPtDefect: "<("MinPtDefect")<points.size()< Config::get("MinPtDefect") ) 402 | { 403 | std::cout<<"cluster has less points than the threshold, drop this cludter!"<("PASS_filter_x_min_offset") || xc >Config::get("PASS_filter_x_max_offset") || 407 | yc < Config::get("PASS_filter_y_min_offset") || yc > Config::get("PASS_filter_y_max_offset") ) 408 | { 409 | std::cout<<"xc,yc,zc: "< target_color_cloudCPn (cloudCPn, 255, 0, 0); 417 | viewer3->addPointCloud (cloudCPn, target_color_cloudCPn, cloud_id); 418 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id); 419 | viewer3->spinOnce(100); 420 | getchar(); 421 | 422 | pcl::PointXYZ min_cloudCPn; 423 | pcl::PointXYZ max_cloudCPn; 424 | pcl::getMinMax3D(*cloudCPn,min_cloudCPn,max_cloudCPn); 425 | 426 | std::cout 427 | <<"min_x: "<("normal_estimate_area_ratio")*std::abs(max_cloudCPn.x-min_cloudCPn.x)/2; 439 | outer_max.x=max_cloudCPn.x+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.x-min_cloudCPn.x)/2; 440 | iner_min.x=min_cloudCPn.x; 441 | iner_max.x=max_cloudCPn.x; 442 | outer_min.y=min_cloudCPn.y-Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.y-min_cloudCPn.y)/2; 443 | outer_max.y=max_cloudCPn.y+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.y-min_cloudCPn.y)/2; 444 | iner_min.y=min_cloudCPn.y; 445 | iner_max.y=max_cloudCPn.y; 446 | outer_min.z=min_cloudCPn.z-Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.z-min_cloudCPn.z)/2; 447 | outer_max.z=max_cloudCPn.z+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.z-min_cloudCPn.z)/2; 448 | iner_min.z=min_cloudCPn.z; 449 | iner_max.z=max_cloudCPn.z; 450 | pcl::Normal avg_normal_estimation_area = getLoopNormal(cloud_filtered, normals,outer_max,outer_min, iner_max,iner_min); 451 | std::cout<<"avg_normal_estimation_area: " 452 | <::Ptr cloud_filtered_CPn(new pcl::PointCloud); 459 | pcl::PassThrough pass; 460 | pass.setInputCloud (cloud); 461 | pass.setFilterFieldName ("x"); 462 | pass.setFilterLimits (outer_min.x,outer_max.x); 463 | std::cout<<"PASS_filter_x_min: "< normal_estimate_area_cloudCPn (cloud_filtered_CPn, 255, 255, 255); 473 | viewer3->addPointCloud (cloud_filtered_CPn, normal_estimate_area_cloudCPn, cloud_id+"_normal_estimate_area"); 474 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id+"_normal_estimate_area"); 475 | viewer3->spinOnce(100); 476 | getchar(); 477 | 478 | pass.setInputCloud (cloud_filtered_CPn); 479 | pass.setFilterFieldName ("x"); 480 | pass.setFilterLimits (min_cloudCPn.x, max_cloudCPn.x); 481 | std::cout<<"PASS_filter_x_min: "< normal_estimate_area_cloudCPn2 (cloud_filtered_CPn, 0, 255, 255); 493 | viewer3->addPointCloud (cloud_filtered_CPn, normal_estimate_area_cloudCPn2, cloud_id+"_normal_estimate_area2"); 494 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id+"_normal_estimate_area2"); 495 | viewer3->spinOnce(100); 496 | getchar(); 497 | 498 | } 499 | } 500 | 501 | 502 | 503 | //可视化，法向量检测到的缺陷 504 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP (cloudCP, 255, 0, 0); 505 | // viewer3->addPointCloud (cloudCP, target_color_cloudCP, "cloudCP"); 506 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP"); 507 | 508 | //可视化，曲率检测到的缺陷 509 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP2 (cloudCP2, 0, 0, 255); 510 | // viewer3->addPointCloud (cloudCP2, target_color_cloudCP2, "cloudCP2"); 511 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP2"); 512 | 513 | //可视化，Z阈值检测到的缺陷 514 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP3 (cloudCP3, 0, 255, 255); 515 | // viewer3->addPointCloud (cloudCP3, target_color_cloudCP3, "cloudCP3"); 516 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP3"); 517 | 518 | while(!viewer3->wasStopped()) 519 | { 520 | viewer3->spinOnce(100); 521 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 522 | } 523 | } 524 | else 525 | { 526 | //创建法线估计的对象 527 | pcl::NormalEstimation normalEstimation; 528 | normalEstimation.setInputCloud(cloud); 529 | //对于每一个点都用半径为3cm的近邻搜索方式 530 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 531 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 532 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 533 | normalEstimation.setSearchMethod(kdtree); 534 | //计算法线 535 | normalEstimation.compute(*normals); 536 | 537 | //可视化 538 | boost::shared_ptr viewer3(new pcl::visualization::PCLVisualizer("Cloud Normals")); 539 | viewer3->setBackgroundColor (0, 0, 0); 540 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 541 | // Coloring and visualizing target cloud (red). 542 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud, 255, 0, 0); 543 | viewer3->addPointCloud (cloud, target_color, "cloud"); 544 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); 545 | pcl::visualization::PointCloudColorHandlerCustom normal_color (normals, 255, 255, 255); 546 | viewer3->addPointCloudNormals(cloud, normals, 1, (0.1*(max.x-min.x)), "normals"); 547 | while(!viewer3->wasStopped()) 548 | { 549 | viewer3->spinOnce(100); 550 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 551 | } 552 | } 553 | return 0; 554 | } -------------------------------------------------------------------------------- /src/DefectDetect_demo.cpp: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | #include 4 | #include 5 | 6 | //boost 7 | #include 8 | 9 | //PCL 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //OpenCV 22 | #include 23 | 24 | //Customed 25 | #include "config.h" 26 | #include "pointcloud_helper.h" 27 | #include "dbscan.h" 28 | #include "DefectDetect.h" 29 | 30 | using namespace std; 31 | using namespace cv; 32 | 33 | #ifndef PI 34 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 35 | #endif 36 | #ifndef pi 37 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 38 | #endif 39 | 40 | int main(int argc,char**argv) 41 | { 42 | string paraFileName = "../parameters/parameter.yml"; 43 | Config::setParameterFile(paraFileName); 44 | 45 | //创建点云对象 46 | pcl::PointCloud::Ptr origin_cloud(new pcl::PointCloud); 47 | 48 | //加载点云数据，命令行参数为文件名，文件可以是.PCD/.pcd/.PLY/.ply四种文件之一，单位可以是mm也可以是m. 49 | string inputFilename=argv[1]; 50 | if(loadPointCloudData(inputFilename, origin_cloud) != 0) 51 | exit(-1); 52 | 53 | process(origin_cloud); 54 | return 0; 55 | } -------------------------------------------------------------------------------- /src/dbscan.cpp: -------------------------------------------------------------------------------- 1 | #include "dbscan.h" 2 | #include 3 | #include 4 | namespace dbscn{ 5 | static const inline double distance(double x1, double y1, double x2, double y2) 6 | { 7 | double dx = x2 - x1; 8 | double dy = y2 - y1; 9 | 10 | return sqrt(dx * dx + dy * dy); 11 | } 12 | 13 | const inline int region_query(const std::vector &input, int p, std::vector &output, double eps) 14 | { 15 | for(int i = 0; i < (int)input.size(); i++){ 16 | 17 | if(distance(input[i].x, input[i].y, input[p].x, input[p].y) < eps){ 18 | output.push_back(i); 19 | } 20 | } 21 | 22 | return output.size(); 23 | } 24 | 25 | bool expand_cluster(const std::vector &input, int p, std::vector &output, int cluster, double eps, int min) 26 | { 27 | std::vector seeds; 28 | 29 | if(region_query(input, p, seeds, eps) < min){ 30 | 31 | //this point is noise 32 | output[p] = -1; 33 | return false; 34 | 35 | }else{ 36 | 37 | //set cluster id 38 | for(int i = 0; i < (int)seeds.size(); i++){ 39 | output[seeds[i]] = cluster; 40 | } 41 | 42 | //delete paint from seeds 43 | seeds.erase(std::remove(seeds.begin(), seeds.end(), p), seeds.end()); 44 | 45 | //seed -> empty 46 | while((int)seeds.size() > 0){ 47 | 48 | int cp = seeds.front(); 49 | std::vector result; 50 | 51 | if(region_query(input, cp, result, eps) >= min){ 52 | 53 | for(int i = 0; i < (int)result.size(); i++){ 54 | 55 | int rp = result[i]; 56 | 57 | //this paint is noise or unmarked point 58 | if(output[rp] < 1){ 59 | 60 | //unmarked point 61 | if(!output[rp]){ 62 | seeds.push_back(rp); 63 | } 64 | 65 | //set cluster id 66 | output[rp] = cluster; 67 | } 68 | } 69 | } 70 | 71 | //delete point from seeds 72 | seeds.erase(std::remove(seeds.begin(), seeds.end(), cp), seeds.end()); 73 | } 74 | } 75 | 76 | return true; 77 | } 78 | 79 | int dbscan(const std::vector &input, std::vector &labels, double eps, int min) 80 | { 81 | int size = input.size(); 82 | int cluster = 1; 83 | 84 | std::vector state(size); 85 | 86 | for(int i = 0; i < size; i++){ 87 | 88 | if(!state[i]){ 89 | 90 | if(expand_cluster(input, i, state, cluster, eps, min)){ 91 | cluster++; 92 | } 93 | } 94 | } 95 | 96 | labels = state; 97 | 98 | return cluster - 1; 99 | } 100 | } -------------------------------------------------------------------------------- /src/demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) 2 | 3 | ######################################## DEMO ############################################## 4 | add_executable(normal_distributions_transform ./normal_distributions_transform.cpp ) 5 | target_link_libraries (normal_distributions_transform ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 6 | 7 | add_executable (iterative_closest_point ./iterative_closest_point.cpp) 8 | target_link_libraries (iterative_closest_point ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 9 | -------------------------------------------------------------------------------- /src/demo/iterative_closest_point.cpp: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | 4 | //PCL 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | //boost 12 | #include 13 | 14 | //OpenCV 15 | #include "opencv2/imgproc/imgproc.hpp" 16 | 17 | int 18 | main (int argc, char** argv) 19 | { 20 | // Loading first scan of room. 21 | pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud); 22 | if (pcl::io::loadPCDFile ("room_scan1.pcd", *target_cloud) == -1) 23 | { 24 | PCL_ERROR ("Couldn't read file room_scan1.pcd \n"); 25 | return (-1); 26 | } 27 | std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl; 28 | 29 | // Loading second scan of room from new perspective. 30 | pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); 31 | if (pcl::io::loadPCDFile ("room_scan2.pcd", *input_cloud) == -1) 32 | { 33 | PCL_ERROR ("Couldn't read file room_scan2.pcd \n"); 34 | return (-1); 35 | } 36 | std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl; 37 | 38 | pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud); 39 | pcl::ApproximateVoxelGrid approximate_voxel_filter; 40 | approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2); 41 | approximate_voxel_filter.setInputCloud (input_cloud); 42 | approximate_voxel_filter.filter (*filtered_cloud); 43 | std::cout << "Filtered cloud contains " << filtered_cloud->size () 44 | << " data points from room_scan2.pcd" << std::endl; 45 | 46 | 47 | 48 | // Set initial alignment estimate found using robot odometry. 49 | Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ()); 50 | Eigen::Translation3f init_translation (1.79387, 0.720047, 0); 51 | Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); 52 | 53 | double start = (double)(cv::getTickCount()); 54 | pcl::IterativeClosestPoint icp; 55 | icp.setMaximumIterations (1000); 56 | icp.setInputSource(filtered_cloud); 57 | icp.setInputTarget(target_cloud); 58 | pcl::PointCloud Final; 59 | icp.align (Final, init_guess); 60 | std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; 61 | std::cout << icp.getFinalTransformation() << std::endl; 62 | double end = ((double)cv::getTickCount() - start) / cv::getTickFrequency(); 63 | std::cout << "所用时间" << end <::Ptr output_cloud (new pcl::PointCloud); 66 | // Transforming unfiltered, input cloud using found transform. 67 | pcl::transformPointCloud (*input_cloud, *output_cloud, icp.getFinalTransformation ()); 68 | 69 | // Saving transformed input cloud. 70 | pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud); 71 | 72 | // Initializing point cloud visualizer 73 | boost::shared_ptr 74 | viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer")); 75 | viewer_final->setBackgroundColor (0, 0, 0); 76 | 77 | // Coloring and visualizing target cloud (red). 78 | pcl::visualization::PointCloudColorHandlerCustom 79 | target_color (target_cloud, 255, 0, 0); 80 | viewer_final->addPointCloud (target_cloud, target_color, "target cloud"); 81 | viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 82 | 1, "target cloud"); 83 | 84 | // Coloring and visualizing transformed input cloud (green). 85 | pcl::visualization::PointCloudColorHandlerCustom 86 | output_color (output_cloud, 0, 255, 0); 87 | viewer_final->addPointCloud (output_cloud, output_color, "output cloud"); 88 | viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 89 | 1, "output cloud"); 90 | 91 | // Starting visualizer 92 | viewer_final->addCoordinateSystem (1.0, 0 ); 93 | // viewer_final->addCoordinateSystem (1.0, "global"); 94 | viewer_final->initCameraParameters (); 95 | 96 | // Wait until visualizer window is closed. 97 | while (!viewer_final->wasStopped ()) 98 | { 99 | viewer_final->spinOnce (100); 100 | boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 101 | } 102 | 103 | 104 | 105 | return (0); 106 | } 107 | -------------------------------------------------------------------------------- /src/demo/normal_distributions_transform.cpp: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | 4 | //PCL 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | //OpenCV 15 | #include "opencv2/imgproc/imgproc.hpp" 16 | 17 | int main (int argc, char** argv) 18 | { 19 | // Loading first scan of room. 20 | pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud); 21 | if (pcl::io::loadPCDFile ("duckmodel.pcd", *target_cloud) == -1) 22 | { 23 | PCL_ERROR ("Couldn't read file duckmodel.pcd \n"); 24 | return (-1); 25 | } 26 | std::cout << "Loaded " << target_cloud->size () << " data points from duckmodel.pcd" << std::endl; 27 | 28 | // Loading second scan of room from new perspective. 29 | pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); 30 | if (pcl::io::loadPCDFile ("duckscenario.pcd", *input_cloud) == -1) 31 | { 32 | PCL_ERROR ("Couldn't read file duckscenario.pcd \n"); 33 | return (-1); 34 | } 35 | std::cout << "Loaded " << input_cloud->size () << " data points from duckscenario.pcd" << std::endl; 36 | 37 | // Filtering input scan to roughly 10% of original size to increase speed of registration. 38 | pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud); 39 | pcl::ApproximateVoxelGrid approximate_voxel_filter; 40 | approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2); 41 | approximate_voxel_filter.setInputCloud (input_cloud); 42 | approximate_voxel_filter.filter (*filtered_cloud); 43 | std::cout << "Filtered cloud contains " << filtered_cloud->size () 44 | << " data points from room_scan2.pcd" << std::endl; 45 | 46 | double start = (double)(cv::getTickCount()); 47 | // Initializing Normal Distributions Transform (NDT). 48 | pcl::NormalDistributionsTransform ndt; 49 | 50 | // Setting scale dependent NDT parameters 51 | // Setting minimum transformation difference for termination condition. 52 | ndt.setTransformationEpsilon (0.01); 53 | // Setting maximum step size for More-Thuente line search. 54 | ndt.setStepSize (0.1); 55 | //Setting Resolution of NDT grid structure (VoxelGridCovariance). 56 | ndt.setResolution (1.0); 57 | 58 | // Setting max number of registration iterations. 59 | ndt.setMaximumIterations (35); 60 | 61 | // Setting point cloud to be aligned. 62 | ndt.setInputSource (filtered_cloud); 63 | // Setting point cloud to be aligned to. 64 | ndt.setInputTarget (target_cloud); 65 | 66 | // Set initial alignment estimate found using robot odometry. 67 | Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ()); 68 | Eigen::Translation3f init_translation (1.79387, 0.720047, 0); 69 | Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); 70 | 71 | // Calculating required rigid transform to align the input cloud to the target cloud. 72 | pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); 73 | double align = (double)(cv::getTickCount()); 74 | std::cout << "所用时间(before align)" << (align-start)/cv::getTickFrequency() < 91 | viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer")); 92 | viewer_final->setBackgroundColor (0, 0, 0); 93 | 94 | // Coloring and visualizing target cloud (red). 95 | pcl::visualization::PointCloudColorHandlerCustom 96 | target_color (target_cloud, 255, 0, 0); 97 | viewer_final->addPointCloud (target_cloud, target_color, "target cloud"); 98 | viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 99 | 1, "target cloud"); 100 | 101 | // Coloring and visualizing transformed input cloud (green). 102 | pcl::visualization::PointCloudColorHandlerCustom 103 | output_color (output_cloud, 0, 255, 0); 104 | viewer_final->addPointCloud (output_cloud, output_color, "output cloud"); 105 | viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 106 | 1, "output cloud"); 107 | 108 | // Starting visualizer 109 | // viewer_final->addCoordinateSystem (1.0, "global"); 110 | viewer_final->addCoordinateSystem (1.0, 0); 111 | viewer_final->initCameraParameters (); 112 | 113 | // Wait until visualizer window is closed. 114 | while (!viewer_final->wasStopped ()) 115 | { 116 | viewer_final->spinOnce (100); 117 | boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 118 | } 119 | 120 | return (0); 121 | } 122 | -------------------------------------------------------------------------------- /src/deprecated/spacialnet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace cv; 14 | using namespace pcl; 15 | 16 | cv::Point project(const pcl::PointXYZ &pt, const cv::Mat &projection_matrix) 17 | { 18 | cv::Mat pt_3D(4, 1, CV_32FC1); 19 | 20 | pt_3D.at(0) = pt.x; 21 | pt_3D.at(1) = pt.y; 22 | pt_3D.at(2) = pt.z; 23 | pt_3D.at(3) = 1.0f; 24 | 25 | cv::Mat pt_2D = projection_matrix * pt_3D; 26 | 27 | float w = pt_2D.at(2); 28 | float x = pt_2D.at(0) / w; 29 | float y = pt_2D.at(1) / w; 30 | return cv::Point(x, y); 31 | } 32 | 33 | void callback(const sensor_msgs::PointCloud2ConstPtr& msg_pc) 34 | { 35 | 36 | // // pcl::PointCloud::Ptr cloudptr (new pcl::PointCloud); 37 | // // if (pcl::io::loadPCDFile (argv[1], *cloudptr) == -1) 38 | // // { 39 | // // PCL_ERROR ("Couldn't read file room_scan1.pcd \n"); 40 | // // return (-1); 41 | // // } 42 | // // std::cout << "Loaded " << cloudptr->width * cloudptr->height << " data points from pcd fields: "<< std::endl; 43 | 44 | // pcl::PointCloud cloud; 45 | // pcl::fromROSMsg(*msg_pc, cloud); 46 | 47 | // // cloud=*cloudptr; 48 | // cv::Mat projection_matrix; 49 | 50 | // float p[12]={671.860209, 0.000000, 643.055016, 0, 0.000000, 671.864080, 374.010332, 0, 0.00, 0.00, 1.0}; 51 | // cv::Mat(3, 4, CV_32FC1, &p).copyTo(projection_matrix); 52 | // cv::Rect frame(0, 0, 640, 480); 53 | // cv::Mat image=Mat::zeros(480,640, CV_8UC3); 54 | 55 | // for(pcl::PointCloud::iterator pt = cloud.points.begin(); pt < cloud.points.end(); pt++) 56 | // { 57 | // if (pt->z < 0) 58 | // { 59 | // continue; 60 | // } 61 | // cv::Point point2d = project(*pt, projection_matrix); 62 | // if (point2d.inside(frame)) 63 | // { 64 | // std::cout<x <<" "<y <<" "<z <(point2d.y,point2d.x)[0]=1000*pt->x; 67 | // // image.at(point2d.y,point2d.x)[1]=1000*pt->y; 68 | // image.at(point2d.y,point2d.x)[2]=1000*pt->z; 69 | // } 70 | 71 | // } 72 | // imshow("image",image); 73 | // waitKey(); 74 | } 75 | 76 | int main (int argc, char** argv) 77 | { 78 | 79 | ros::init(argc, argv, "spacialnet"); 80 | // ros::NodeHandle n; 81 | // ROS_INFO_STREAM("haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha ^_^ haha"); 82 | 83 | // ros::Subscriber sub = n.subscribe("/camera/depth_registered/points", 10, callback); 84 | 85 | // // message_filters::Subscriber cloud_sub(n, "/camera/depth_registered/points", 1); 86 | // // message_filters::Subscriber image_sub(n, "/camera/rgb/image_rect_color", 1); 87 | 88 | // // typedef sync_policies::ApproximateTime MySyncPolicy; 89 | // // Synchronizer sync(MySyncPolicy(10), cloud_sub, image_sub); 90 | // // sync.registerCallback(boost::bind(&callback, _1, _2)); 91 | // ros::spin(); 92 | 93 | return 0; 94 | } -------------------------------------------------------------------------------- /src/doc/pcl help functions.cpp: -------------------------------------------------------------------------------- 1 | pcl::PassThrough pass; 2 | pass.setInputCloud (cloud); //设置输入点云 3 | pass.setFilterFieldName ("z"); //设置过滤时所需要点云类型的Z字段 4 | pass.setFilterLimits (0.0, 1.0); //设置在过滤字段的范围 5 | //pass.setFilterLimitsNegative (true); //设置保留范围内还是过滤掉范围内 6 | pass.filter (*cloud_filtered); 7 | 8 | 9 | //the mapping tells you to that points of the oldcloud the new ones correspond 10 | //but we will not use it 11 | std::vector mapping; 12 | pcl::removeNaNFromPointCloud(_p_cloud, _p_cloud, mapping); 13 | std::cout<<"point size: "<<_p_cloud.points.size()<::Ptr input_cloud (new pcl::PointCloud); 20 | pcl::PLYReader::read("duckscenario.ply", *input_cloud, 0); 21 | std::cout << "Loaded " << input_cloud->size () << " data points from duckscenario.ply" << std::endl; 22 | 23 | 24 | 25 | //可视化 26 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Cloud Original")); 27 | pcl::visualization::PointCloudColorHandlerCustom single_color (cloud, 0, 255, 0); 28 | viewer->addPointCloud(cloud, single_color, "Original"); 29 | viewer->addCoordinateSystem(0.1*(max.y-min.y)); 30 | while(!viewer->wasStopped()) 31 | { 32 | viewer->spinOnce(100); 33 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 34 | } 35 | viewer->removePointCloud("cloud"); 36 | viewer->removeAllPointClouds(); 37 | viewer->close(); 38 | 39 | 40 | cv::Mat image=cv::Mat::zeros(1080, 1920, CV_8UC3); 41 | for(pcl::PointCloud::iterator pt = cloud_filtered->points.begin(); pt < cloud_filtered->points.end(); pt++) 42 | { 43 | std::cout<x <<" "<y <<" "<z <y)<<" "<x)<<" "<< int(5000*pt->y)+500 <<" "<< int(5000*pt->x)+500 <(int(5000*pt->y)+500,int(5000*pt->x)+500)[0]=int(5000*pt->x); 46 | image.at(int(5000*pt->y)+500,int(5000*pt->x)+500)[1]=int(5000*pt->y); 47 | image.at(int(5000*pt->y)+500,int(5000*pt->x)+500)[2]=int(5000*pt->z); 48 | 49 | std::cout<x <<" "<y <<" "<z <y)<<" "<x)<<" "<< int(1000*pt->y)+100 <<" "<< int(1000*pt->x)+100 <(int(1000*pt->y)+100,int(1000*pt->x)+100)[0]=int(1000*pt->x); 52 | image.at(int(1000*pt->y)+100,int(1000*pt->x)+100)[1]=int(1000*pt->y); 53 | image.at(int(1000*pt->y)+100,int(1000*pt->x)+100)[2]=int(1000*pt->z); 54 | } 55 | cv::imshow("image",image); 56 | cv::waitKey(); 57 | cv::imwrite("image.png",image); -------------------------------------------------------------------------------- /src/fit_plane_from_file.cpp: -------------------------------------------------------------------------------- 1 | //C++ 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | //OpenCV 12 | #include 13 | 14 | //PCL 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | //Eigen 30 | #include 31 | 32 | //Customed 33 | #include 34 | 35 | #ifndef PI 36 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 37 | #endif 38 | #ifndef pi 39 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 40 | #endif 41 | 42 | using namespace cv; 43 | using namespace std; 44 | using namespace pcl; 45 | 46 | 47 | int main(int argc, char** argv) 48 | { 49 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 50 | string inputFilename=argv[1]; 51 | if(loadPointCloudData(inputFilename, cloud) != 0) 52 | exit(-1); 53 | cv::Mat cv_R = getR2registeZ(cloud); 54 | return 0; 55 | } -------------------------------------------------------------------------------- /src/libs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) 2 | 3 | ######################################## TOOLS ############################################## 4 | 5 | find_library(RUNTIME_LIB config) 6 | message(${RUNTIME_LIB}) 7 | 8 | add_library(config SHARED config.cc) 9 | add_library(Utility SHARED Utility.cc) 10 | add_library(pointcloud_helper SHARED pointcloud_helper.cc) 11 | 12 | 13 | ######################################## install ############################################## 14 | INSTALL(TARGETS config Utility 15 | RUNTIME DESTINATION bin 16 | LIBRARY DESTINATION lib 17 | ARCHIVE DESTINATION libstatic 18 | ) 19 | -------------------------------------------------------------------------------- /src/libs/Utility.cc: -------------------------------------------------------------------------------- 1 | #include "Utility.h" 2 | 3 | 4 | /*---------------------------- 5 | * 功能 : 判断生成的随机数点，是否重复 6 | *---------------------------- 7 | * 函数 : check 8 | * 返回 : bool，true表示重复，false表示不重复 9 | * 10 | * 参数 : index [in] 已生成的点下标，vector 11 | * 参数 : k [in] 需要比较的点 12 | */ 13 | bool check(vector> points, pair new_points) 14 | { 15 | for(int i = 0; i < points.size(); i++) 16 | { 17 | if( new_points.first == points[i].first && new_points.second == points[i].second) 18 | return true; 19 | else if(new_points.second == points[i].first && new_points.first == points[i].second) 20 | return true; 21 | } 22 | return false; 23 | } 24 | /*---------------------------- 25 | * 功能 : 输入样本点，返回拟合直线参数 26 | *---------------------------- 27 | * 函数 : getLine 28 | * 返回 : 拟合直线参数：a0 a1 29 | * 30 | * 参数 : samplePoints [in] 样本，vector 31 | * 参数 : sampleWeights [in] 样本权重，vector 32 | * 参数 : Mat(2*1) [out] a0 a1,存储在Mat中 33 | */ 34 | template 35 | Mat getLine(vector samplePoints,vector sampleWeights = vector(1) ) 36 | { 37 | Mat left = (Mat_(2,2) << 0, 0, 0, 0); 38 | Mat right = (Mat_(2,1) << 0, 0); 39 | 40 | if(sampleWeights.size() == 1) 41 | sampleWeights = vector(samplePoints.size(),1); 42 | 43 | for(int i = 0; i < samplePoints.size();i ++) 44 | { 45 | left.at(0,0) += sampleWeights[i]; 46 | left.at(0,1) += (sampleWeights[i]) * i; 47 | left.at(1,0) += (sampleWeights[i]) * i; 48 | left.at(1,1) += (sampleWeights[i]) * i * i; 49 | right.at(0,0) += (sampleWeights[i]) * (samplePoints[i]); 50 | right.at(1,0) += (sampleWeights[i]) * (samplePoints[i]) * i; 51 | 52 | } 53 | return left.inv() * right; 54 | } 55 | 56 | /*---------------------------- 57 | * 功能 : 计算点到直线距离 58 | *---------------------------- 59 | * 函数 : getDistance 60 | * 返回 : 点到直线距离 61 | * 62 | * 参数 : y [in] 点y坐标 63 | * 参数 : x [in] 点x坐标（vector下标） 64 | * 参数 : line [in] 直线参数（Mat） 65 | * 参数 : distance [out]点到直线距离 66 | */ 67 | double getDistance(double x,double y,Mat line) 68 | { 69 | return abs(line.at(1) * x - y + line.at(0)) / sqrt(line.at(1) * line.at(1) +1); 70 | } 71 | /*---------------------------- 72 | * 功能 : Ransac 73 | *---------------------------- 74 | * 函数 : ransac 75 | * 返回 : 拟合直线参数 76 | * 77 | * 参数 : numOfp [in] 观测样本点数 78 | * 参数 : samples [in] 样本vector 79 | * 参数 : k [in] 迭代次数 80 | * 参数 : threshold_In [in] 判断为内点的阈值 81 | * 参数 : average [in] 内点取值均值，引用 82 | * 参数 : Result [in] 拟合直线参数（Mat） 83 | */ 84 | 85 | /*---------------------------- 86 | * 功能 : Ransac 87 | *---------------------------- 88 | * 函数 : ransac 89 | * 返回 : 拟合直线参数 90 | * 91 | * 参数 : num [in] 观测样本点数 92 | * 参数 : samples [in] 样本vector 93 | * 参数 : max_iteration [in] 迭代次数 94 | * 参数 : threshold_In [in] 判断为内点的阈值 95 | * 参数 : average [in] 内点取值均值，引用 96 | * 参数 : Result [in] 拟合直线参数（Mat） 97 | */ 98 | 99 | Mat ransac(int num, std::vector samples, int max_iteration,double threshold_In, double& average) 100 | { 101 | RNG rng; //随机数生成器 102 | vector weights(num,1); 103 | // double threshold_In = 1; //判断为内点的距离阈值 104 | // int max_iteration = 200; //迭代次数 105 | int num_ofInliners = 0; //内点数目 106 | Mat Result = (Mat_(2,1) << 0, 0); //迭代结果直线 107 | 108 | if (samples.size() < 2) 109 | { 110 | cout<<"Too little samples!"<> randomP_array; //随机点对容器 115 | vector p1_array; //随机选取点容器 1 116 | vector p2_array; //随机选取点容器 2 117 | 118 | for(int i = 0; i < max_iteration; i++) 119 | { 120 | // 随机选取两点，计算方程 121 | int p1 = 0; 122 | int p2 = 0; 123 | pair p(0,0); 124 | while(p1 == p2 || check(randomP_array,p)) 125 | { 126 | p1 = rng.uniform(0,num); //随机选取两个点（下标） 127 | p2 = rng.uniform(0,num); 128 | } 129 | p.first = p1; 130 | p.second = p2; 131 | randomP_array.push_back(p); 132 | vector linePara; 133 | vector lineWeight; 134 | linePara.push_back(samples[p1]); 135 | linePara.push_back(samples[p2]); 136 | lineWeight.push_back(1); 137 | lineWeight.push_back(1); 138 | Mat x = (Mat_(2,1) << 0, 0); 139 | x = getLine(linePara,lineWeight); 140 | 141 | // 计算点到直线距离，判断是否为内点 142 | vector inliners; //内点容器 143 | for(int j = 0; j < num; j++) 144 | { 145 | if(getDistance(j,samples[j],x) < threshold_In) 146 | inliners.push_back(samples[j]); 147 | } 148 | // 根据内点重新估计模型 149 | Mat lineTemp = getLine(inliners); 150 | if(inliners.size() > num_ofInliners) 151 | { 152 | Result = lineTemp; 153 | num_ofInliners = inliners.size(); 154 | //计算内点取值均值 155 | double sum = 0; 156 | for (vector::iterator iter = inliners.begin();iter != inliners.end();iter++) 157 | { 158 | sum += *iter; 159 | } 160 | average = sum / inliners.size();; 161 | } 162 | } 163 | return Result; 164 | } -------------------------------------------------------------------------------- /src/libs/config.cc: -------------------------------------------------------------------------------- 1 | #include "../../include/config.h" 2 | 3 | void Config::setParameterFile(const std::string& fileName) 4 | { 5 | if (config_ == NULL) 6 | config_ = new Config; //std::shared_ptr(new Config); 7 | config_->file_ = cv::FileStorage(fileName.c_str(), cv::FileStorage::READ); 8 | if (!config_->file_.isOpened()){ 9 | std::cerr << "parameter file" << fileName << "does not exist." << std::endl; 10 | config_->file_.release(); 11 | return; 12 | } 13 | } 14 | 15 | Config::~Config() 16 | { 17 | if (file_.isOpened()) 18 | file_.release(); 19 | delete config_; 20 | } 21 | 22 | Config* Config::config_ = NULL; -------------------------------------------------------------------------------- /src/libs/pointcloud_helper.cc: -------------------------------------------------------------------------------- 1 | //Customed 2 | #include 3 | 4 | #ifndef PI 5 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 6 | #endif 7 | #ifndef pi 8 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 9 | #endif 10 | 11 | using namespace cv; 12 | using namespace std; 13 | using namespace pcl; 14 | 15 | cv::Mat getR2registeZ(pcl::PointCloud::Ptr cloud) 16 | { 17 | double start = (double)(cv::getTickCount()); 18 | 19 | //事实说明：在for循环内重新定义平面拟合实例和在for循环外重新定义其他名称的平面拟合实例都说明了：点云不变的情况下，平面方程系数是不会变化的。 20 | std::ofstream outfile_a ("./plane equation A.txt",std::ios_base::trunc); 21 | std::ofstream outfile_b ("./plane equation B.txt",std::ios_base::trunc); 22 | std::ofstream outfile_c ("./plane equation C.txt",std::ios_base::trunc); 23 | std::ofstream outfile_d ("./plane equation D.txt",std::ios_base::trunc); 24 | 25 | //fit plane equation 26 | // 保存局内点索引 27 | std::vector inliers; 28 | //平面方程系数 29 | Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1); 30 | Eigen::VectorXf coef_opt = Eigen::VectorXf::Zero(4 , 1); 31 | Eigen::VectorXf coef_opt2 = Eigen::VectorXf::Zero(4 , 1); 32 | // 采样一致性模型对象 33 | pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(cloud)); 34 | pcl::RandomSampleConsensus ransac(model_p); 35 | ransac.setDistanceThreshold(0.01); 36 | ransac.computeModel(); 37 | ransac.getInliers(inliers); 38 | 39 | ransac.getModelCoefficients(coef); 40 | outfile_a<optimizeModelCoefficients(inliers,coef,coef_opt); 56 | outfile_a<optimizeModelCoefficients(inliers,coef_opt,coef_opt2); 62 | outfile_a<::Ptr cloud) 116 | { 117 | double start = (double)(cv::getTickCount()); 118 | 119 | string pcd=".pcd"; 120 | string ply=".ply"; 121 | string PCD=".PCD"; 122 | string PLY=".PLY"; 123 | string::size_type id1; 124 | string::size_type id2; 125 | 126 | 127 | id1=inputFilename.find(pcd);//在inputFilename中查找pcd. 128 | if(id1 == string::npos )//不存在。 129 | { 130 | id1=inputFilename.find(PCD); 131 | if(id1 == string::npos ) 132 | { 133 | std::cout<<"inputFile is not .pcd file."<(inputFilename,*cloud) !=0) 139 | { 140 | return -1; 141 | } 142 | } 143 | } 144 | else//存在。 145 | { 146 | //读取PCD文件 147 | if(pcl::io::loadPCDFile(inputFilename,*cloud) !=0) 148 | { 149 | return -1; 150 | } 151 | } 152 | 153 | id2=inputFilename.find(ply);//在inputFilename中查找pcd. 154 | if(id2 == string::npos )//不存在。 155 | { 156 | id2=inputFilename.find(PLY); 157 | if(id2 == string::npos ) 158 | { 159 | std::cout<<"inputFile is not .ply file."<(inputFilename,*cloud) !=0) 165 | { 166 | return -1; 167 | } 168 | } 169 | } 170 | else//存在。 171 | { 172 | //读取PCD文件 173 | if(pcl::io::loadPLYFile(inputFilename,*cloud) !=0) 174 | { 175 | return -1; 176 | } 177 | } 178 | 179 | if( (id1 == string::npos) && (id2 == string::npos) ) 180 | { 181 | double end = ((double)cv::getTickCount() - start) / cv::getTickFrequency(); 182 | std::cout << "loadPointCloudData() 所用时间为：" << end <::Ptr& cloud, float &xc, float &yc, float &zc) 208 | { 209 | float sum_x = 0; 210 | float sum_y = 0; 211 | float sum_z = 0; 212 | for(pcl::PointCloud::iterator pt = cloud->points.begin(); pt < cloud->points.end(); pt++) 213 | { 214 | sum_x = sum_x+ pt->x;sum_y = sum_y+ pt->y;sum_z = sum_z+ pt->z; 215 | } 216 | xc = sum_x/cloud->points.size(); 217 | yc = sum_y/cloud->points.size(); 218 | zc = sum_z/cloud->points.size(); 219 | } 220 | 221 | 222 | void getMinMax3D(pcl::PointCloud::Ptr& cloud, 223 | float &min_x, float &max_x, float &min_y, 224 | float &max_y, float &min_z, float &max_z) 225 | { 226 | for(pcl::PointCloud::iterator pt = cloud->points.begin(); pt < cloud->points.end(); pt++) 227 | { 228 | if (min_x > pt->x) 229 | min_x = pt->x; 230 | if (max_x < pt->x) 231 | max_x = pt->x; 232 | if (min_y > pt->y) 233 | min_y = pt->y; 234 | if (max_y < pt->y) 235 | max_y = pt->y; 236 | if (min_z > pt->z) 237 | min_z = pt->z; 238 | if (max_z < pt->z) 239 | max_z = pt->z; 240 | } 241 | } 242 | 243 | 244 | Eigen::Matrix4f planeAlign(const pcl::PointCloud::Ptr& cloud) 245 | { 246 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 247 | Eigen::Matrix4f tf; 248 | double a, b, c; 249 | Eigen::MatrixXf data = Eigen::MatrixXf::Zero(cloud->width, 3); 250 | tf << 1, 0, 0, 0, 251 | 0, 1, 0, 0, 252 | 0, 0, 1, 0, 253 | 0, 0, 0, 1; 254 | 255 | Eigen::Vector3f mean; 256 | mean.fill(0); 257 | for(int i = 0; i < cloud->width; i++) 258 | { 259 | mean(0) += cloud->points[i].x; 260 | mean(1) += cloud->points[i].y; 261 | mean(2) += cloud->points[i].z; 262 | } 263 | mean /= cloud->width; 264 | for(int i = 0; i < cloud->width; i++) 265 | { 266 | data(i, 0) = cloud->points[i].x - mean(0); 267 | data(i, 1) = cloud->points[i].y - mean(1); 268 | data(i, 2) = cloud->points[i].z - mean(2); 269 | } 270 | 271 | Eigen::JacobiSVD svd(data, Eigen::ComputeThinU | Eigen::ComputeThinV); 272 | 273 | Eigen::Matrix3f V; 274 | V = svd.matrixV(); 275 | a = V(0, 2); 276 | b = V(1, 2); 277 | c = V(2, 2); 278 | //cout< distance; 291 | double temp; 292 | 293 | for(int i = 0; i < cloud->width; i++) 294 | { 295 | temp = abs(a * cloud->points[i].x + b * cloud->points[i].y + c * cloud->points[i].z + dis) / sqrt(pow(a, 2) + pow(b, 2) + pow(c, 2)); 296 | distance.push_back(temp); 297 | } 298 | 299 | sort(distance.begin(), distance.end()); 300 | 301 | cout << "MAX distance between pointcloud with the plane : " << distance[distance.size() - 1] << endl; 302 | return tf; 303 | } 304 | 305 | pcl::Normal getLoopNormal( 306 | const pcl::PointCloud::Ptr& cloud, 307 | const pcl::PointCloud::Ptr& normal, 308 | pcl::PointXYZ& outer_max, 309 | pcl::PointXYZ& outer_min, 310 | pcl::PointXYZ& iner_max, 311 | pcl::PointXYZ& iner_min) 312 | { 313 | pcl::Normal avg_normal; 314 | float nx=0; 315 | float ny=0; 316 | float nz=0; 317 | int count=0; 318 | 319 | for (size_t i = 0; i < cloud->points.size(); ++i) 320 | { 321 | if ((cloud->points[i].x < outer_max.x) && (cloud->points[i].x > iner_max.x)) 322 | { 323 | if ((cloud->points[i].y < outer_max.y) && (cloud->points[i].y > outer_min.y)) 324 | { 325 | nx=nx+normal->points[i].normal_x; 326 | ny=ny+normal->points[i].normal_y; 327 | nz=nz+normal->points[i].normal_z; 328 | std::cout<<" x, y, z: "<points[i].x<<" "<points[i].y<<" "<points[i].z<points[i].x < iner_min.x) && (cloud->points[i].x > outer_min.x)) 333 | { 334 | if ((cloud->points[i].y < outer_max.y) && (cloud->points[i].y > outer_min.y)) 335 | { 336 | nx=nx+normal->points[i].normal_x; 337 | ny=ny+normal->points[i].normal_y; 338 | nz=nz+normal->points[i].normal_z; 339 | std::cout<<" x, y, z: "<points[i].x<<" "<points[i].y<<" "<points[i].z<points[i].x < iner_max.x) && (cloud->points[i].x > iner_min.x)) 344 | { 345 | if ((cloud->points[i].y < outer_max.y) && (cloud->points[i].y > iner_max.y)) 346 | { 347 | nx=nx+normal->points[i].normal_x; 348 | ny=ny+normal->points[i].normal_y; 349 | nz=nz+normal->points[i].normal_z; 350 | std::cout<<" x, y, z: "<points[i].x<<" "<points[i].y<<" "<points[i].z<points[i].y < iner_min.y) && (cloud->points[i].y > outer_min.y)) 354 | { 355 | nx=nx+normal->points[i].normal_x; 356 | ny=ny+normal->points[i].normal_y; 357 | nz=nz+normal->points[i].normal_z; 358 | std::cout<<" x, y, z: "<points[i].x<<" "<points[i].y<<" "<points[i].z< 3 | #include 4 | #include 5 | 6 | //boost 7 | #include 8 | 9 | //PCL 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //OpenCV 22 | #include 23 | 24 | //Customed 25 | #include "config.h" 26 | #include "pointcloud_helper.h" 27 | #include "dbscan.h" 28 | 29 | 30 | using namespace std; 31 | using namespace cv; 32 | 33 | #ifndef PI 34 | #define PI 3.1415926535897932384626433832795028841971693993751058209749445 35 | #endif 36 | #ifndef pi 37 | #define pi 3.1415926535897932384626433832795028841971693993751058209749445 38 | #endif 39 | 40 | 41 | 42 | int main(int argc,char**argv) 43 | { 44 | string paraFileName = "../parameters/parameter.yml"; 45 | Config::setParameterFile(paraFileName); 46 | 47 | //创建点云对象 48 | pcl::PointCloud::Ptr origin_cloud(new pcl::PointCloud); 49 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 50 | //创建法线的对象 51 | pcl::PointCloud::Ptr normals(new pcl::PointCloud); 52 | //记录点云对齐的缸体变换矩阵 53 | Eigen::Matrix4f tf; 54 | 55 | //加载点云数据，命令行参数为文件名，文件可以是.PCD/.pcd/.PLY/.ply四种文件之一，单位可以是mm也可以是m. 56 | string inputFilename=argv[1]; 57 | if(loadPointCloudData(inputFilename, origin_cloud) != 0) 58 | exit(-1); 59 | pcl::copyPointCloud(*origin_cloud, *cloud); 60 | 61 | //计算点云分布范围，如果分布范围大于1，说明单位是mm，小于1，说明单位是m. 62 | //用于存放三个轴的最小值 63 | pcl::PointXYZ min; 64 | //用于存放三个轴的最大值 65 | pcl::PointXYZ max; 66 | pcl::getMinMax3D(*cloud,min,max); 67 | std::cout<<"min.x = "<::iterator pt = cloud->points.begin(); pt < cloud->points.end(); pt++) 82 | { 83 | pt->x =1000*pt->x; 84 | pt->y =1000*pt->y; 85 | pt->z =1000*pt->z; 86 | } 87 | max.x = 1000*max.x; 88 | min.x = 1000*min.x; 89 | max.y = 1000*max.y; 90 | min.y = 1000*min.y; 91 | max.z = 1000*max.z; 92 | min.z = 1000*min.z; 93 | std::cout<<"min.x = "<setBackgroundColor (0, 0, 0); 100 | viewer->addCoordinateSystem(0.3*(max.y-min.y)); 101 | pcl::visualization::PointCloudColorHandlerCustom single_color (cloud, 0, 255, 0); 102 | viewer->addPointCloud(cloud, single_color, "Original"); 103 | while(!viewer->wasStopped()) 104 | { 105 | viewer->spinOnce(100); 106 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 107 | } 108 | 109 | 110 | 111 | 112 | //对点云进行平面拟合（可选先进行提速滤波和带通滤波以加快处理速度），并且将点云旋转到传感器轴与测量平面垂直 113 | if (Config::get("VoxelGrid_filter_before_fitting_plane")) 114 | { 115 | std::cout<<"VoxelGrid_filter_before_fitting_plane: "<("VoxelGrid_filter_before_fitting_plane")<::Ptr cloud_filtered(new pcl::PointCloud); 117 | pcl::VoxelGrid vg; 118 | //创建滤波对象 119 | vg.setInputCloud (cloud); 120 | //设置需要过滤的点云给滤波对象 121 | vg.setLeafSize ( 122 | Config::get("VoxelGrid_filter_before_fitting_plane_x"), 123 | Config::get("VoxelGrid_filter_before_fitting_plane_y"), 124 | Config::get("VoxelGrid_filter_before_fitting_plane_z")); 125 | //设置滤波时创建的体素体积为1cm的立方体 126 | vg.filter (*cloud_filtered); 127 | //执行滤波 128 | 129 | cv::Mat cv_R = getR2registeZ(cloud_filtered); 130 | Eigen::Matrix4f tf; 131 | for (int i = 0; i < 3; ++i) 132 | { 133 | for (int j = 0; j < 3; ++j) 134 | { 135 | tf(i,j)=cv_R.at(i,j); 136 | } 137 | } 138 | for (int i = 0; i < 4; ++i) 139 | { 140 | tf(i,3) = 0; 141 | tf(3,i) = 0; 142 | } 143 | tf(3,3)=1; 144 | std::cout<<"tf Matrix4f:\n"<(i,j); 157 | } 158 | } 159 | for (int i = 0; i < 4; ++i) 160 | { 161 | tf(i,3) = 0; 162 | tf(3,i) = 0; 163 | } 164 | tf(3,3)=1; 165 | std::cout<<"tf Matrix4f:\n"< viewer2(new pcl::visualization::PCLVisualizer("Cloud Transformed")); 171 | viewer2->setBackgroundColor (0, 0, 0); 172 | viewer2->addCoordinateSystem(0.3*(max.y-min.y)); 173 | pcl::visualization::PointCloudColorHandlerCustom single_color2 (cloud, 0, 255, 0); 174 | viewer2->addPointCloud(cloud, single_color, "Transformed"); 175 | while(!viewer2->wasStopped()) 176 | { 177 | viewer2->spinOnce(100); 178 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 179 | } 180 | 181 | 182 | 183 | 184 | if (Config::get("VoxelGrid_filter") && !Config::get("PASS_filter")) 185 | { 186 | std::cout<<"VoxelGrid_filter: "<("VoxelGrid_filter")<::Ptr cloud_filtered(new pcl::PointCloud); 188 | pcl::VoxelGrid vg; 189 | //创建滤波对象 190 | vg.setInputCloud (cloud); 191 | //设置需要过滤的点云给滤波对象 192 | vg.setLeafSize (Config::get("VoxelGrid_x"), Config::get("VoxelGrid_y"), Config::get("VoxelGrid_z")); 193 | //设置滤波时创建的体素体积为1cm的立方体 194 | vg.filter (*cloud_filtered); 195 | //执行滤波 196 | 197 | 198 | 199 | //创建法线估计的对象 200 | pcl::NormalEstimation normalEstimation; 201 | normalEstimation.setInputCloud(cloud_filtered); 202 | //对于每一个点都用半径为3cm的近邻搜索方式 203 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 204 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 205 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 206 | normalEstimation.setSearchMethod(kdtree); 207 | //计算法线 208 | normalEstimation.compute(*normals); 209 | 210 | 211 | 212 | //可视化 213 | boost::shared_ptr viewer3(new pcl::visualization::PCLVisualizer("Cloud Normals")); 214 | viewer3->setBackgroundColor (0, 0, 0); 215 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 216 | // Coloring and visualizing target cloud (red). 217 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud_filtered, 255, 0, 0); 218 | viewer3->addPointCloud (cloud_filtered, target_color, "cloud_filtered"); 219 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_filtered"); 220 | viewer3->addPointCloudNormals(cloud_filtered, normals, 1, (0.1*(max.x-min.x)), "normals"); 221 | while(!viewer3->wasStopped()) 222 | { 223 | viewer3->spinOnce(100); 224 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 225 | } 226 | } 227 | else if (Config::get("PASS_filter") && !Config::get("VoxelGrid_filter")) 228 | { 229 | std::cout<<"PASS_filter: "<("PASS_filter")<::Ptr cloud_filtered(new pcl::PointCloud); 231 | pcl::PassThrough pass; 232 | pass.setInputCloud (cloud); 233 | pass.setFilterFieldName (Config::get("PASS_filter_x")); 234 | pass.setFilterLimits (Config::get("PASS_filter_x_min"), Config::get("PASS_filter_x_max")); 235 | std::cout<<"PASS_filter_x_min: "<("PASS_filter_x_min")<("PASS_filter_x_max")<("PASS_filter_y")); 240 | pass.setFilterLimits (Config::get("PASS_filter_y_min"), Config::get("PASS_filter_y_max")); 241 | std::cout<<"PASS_filter_y_min: "<("PASS_filter_y_min")<("PASS_filter_y_max")< normalEstimation; 249 | normalEstimation.setInputCloud(cloud_filtered); 250 | //对于每一个点都用半径为3cm的近邻搜索方式 251 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 252 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 253 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 254 | normalEstimation.setSearchMethod(kdtree); 255 | //计算法线 256 | normalEstimation.compute(*normals); 257 | 258 | 259 | 260 | 261 | /* Detect defect using normal_z*/ 262 | float max_theta_to_ZAxis = Config::get("max_theta_to_ZAxis"); 263 | std::cout<<"max_theta_to_ZAxis is: "< index; 266 | int tmp_index=0; 267 | 268 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 269 | { 270 | if (std::abs(pt->normal_z) < abs_z_expect) 271 | { 272 | index.push_back(tmp_index); 273 | std::cout<<"index: "<normal_x <<" normal_y: "<normal_y 274 | <<" normal_z: "<normal_z <<" abs_z_expect: "<::Ptr cloudCP(new pcl::PointCloud); 279 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP); 280 | 281 | 282 | 283 | 284 | /* Detect defect using curvature*/ 285 | tmp_index =0; 286 | index.clear(); 287 | float sum_curvature=0; 288 | float max_curvature=0; 289 | float avg_curvature=0; 290 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 291 | { 292 | sum_curvature =sum_curvature + pt->curvature; 293 | if (max_curvature < pt->curvature) 294 | { 295 | max_curvature = pt->curvature; 296 | } 297 | } 298 | avg_curvature = sum_curvature/normals->points.size(); 299 | // float curvature_expect = avg_curvature*Config::get("curvature_expect_overshot_ratio"); 300 | float curvature_expect = max_curvature-(max_curvature-avg_curvature)*Config::get("curvature_expect_down_ratio"); 301 | 302 | for(pcl::PointCloud::iterator pt = normals->points.begin(); pt < normals->points.end(); pt++) 303 | { 304 | if (std::abs(pt->curvature) > curvature_expect) 305 | { 306 | index.push_back(tmp_index); 307 | std::cout<<"index: "<curvature<<" curvature_expect: "<::Ptr cloudCP2(new pcl::PointCloud); 312 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP2); 313 | 314 | 315 | 316 | 317 | /* Detect defect using pointcloud_z*/ 318 | float sum_z=0; 319 | float max_z=0; 320 | float avg_z=0; 321 | for(pcl::PointCloud::iterator pt = cloud_filtered->points.begin(); pt < cloud_filtered->points.end(); pt++) 322 | { 323 | sum_z = sum_z+ pt->z; 324 | if (max_z < pt->z) 325 | { 326 | max_z = pt->z; 327 | } 328 | } 329 | avg_z = sum_z/cloud_filtered->points.size(); 330 | tmp_index =0; 331 | index.clear(); 332 | float expect_z_overshot = avg_z + Config::get("expect_z_overshot"); 333 | 334 | for(pcl::PointCloud::iterator pt = cloud_filtered->points.begin(); pt < cloud_filtered->points.end(); pt++) 335 | { 336 | if (std::abs(pt->z) > expect_z_overshot) 337 | { 338 | index.push_back(tmp_index); 339 | std::cout<<"index: "<z: "<< pt->z<<" expect_z_overshot: "<::Ptr cloudCP3(new pcl::PointCloud); 344 | pcl::copyPointCloud(*cloud_filtered, index, *cloudCP3); 345 | 346 | 347 | 348 | /*Cluster using DBSCN*/ 349 | std::vector points; 350 | dbscn::Point tmp_pt; 351 | for (size_t i = 0; i < cloudCP->points.size(); ++i) 352 | { 353 | tmp_pt.x=cloudCP->points[i].x; 354 | tmp_pt.y=cloudCP->points[i].y; 355 | points.push_back(tmp_pt); 356 | } 357 | vector labels; 358 | int num_cluster = dbscn::dbscan(points, labels, Config::get("exp"), Config::get("MinPt")); 359 | cout<<"cluster size is "< viewer3(new pcl::visualization::PCLVisualizer("Defect Dtect Results")); 370 | viewer3->setBackgroundColor (0, 0, 0); 371 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 372 | if (Config::get("show_normal")) 373 | { 374 | viewer3->addPointCloudNormals(cloud_filtered, normals, 1, (0.1*(max.x-min.x)), "normals"); 375 | } 376 | // pcl::visualization::PointCloudColorHandlerCustom cloud_color (cloud_filtered, 125, 125, 125); 377 | // viewer3->addPointCloud (cloud, cloud_color, "cloud"); 378 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud_filtered, 0, 255, 0); 379 | viewer3->addPointCloud (cloud_filtered, target_color, "cloud_filtered"); 380 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_filtered"); 381 | 382 | for (int i = 1; i <= num_cluster; i++) 383 | { 384 | stringstream ss; 385 | ss<::Ptr cloudCPn(new pcl::PointCloud); 395 | pcl::copyPointCloud(*cloudCP, index, *cloudCPn); 396 | std::string cloud_id="cloudCP_"+ss.str(); 397 | std::cout<<"\ncloud_id: "<points.size() <<", MinPtDefect: "<("MinPtDefect")<points.size()< Config::get("MinPtDefect") ) 401 | { 402 | std::cout<<"cluster has less points than the threshold, drop this cludter!"<("PASS_filter_x_min_offset") || xc >Config::get("PASS_filter_x_max_offset") || 406 | yc < Config::get("PASS_filter_y_min_offset") || yc > Config::get("PASS_filter_y_max_offset") ) 407 | { 408 | std::cout<<"xc,yc,zc: "< target_color_cloudCPn (cloudCPn, 255, 0, 0); 416 | viewer3->addPointCloud (cloudCPn, target_color_cloudCPn, cloud_id); 417 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id); 418 | viewer3->spinOnce(100); 419 | getchar(); 420 | 421 | pcl::PointXYZ min_cloudCPn; 422 | pcl::PointXYZ max_cloudCPn; 423 | pcl::getMinMax3D(*cloudCPn,min_cloudCPn,max_cloudCPn); 424 | 425 | std::cout 426 | <<"min_x: "<("normal_estimate_area_ratio")*std::abs(max_cloudCPn.x-min_cloudCPn.x)/2; 438 | outer_max.x=max_cloudCPn.x+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.x-min_cloudCPn.x)/2; 439 | iner_min.x=min_cloudCPn.x; 440 | iner_max.x=max_cloudCPn.x; 441 | outer_min.y=min_cloudCPn.y-Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.y-min_cloudCPn.y)/2; 442 | outer_max.y=max_cloudCPn.y+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.y-min_cloudCPn.y)/2; 443 | iner_min.y=min_cloudCPn.y; 444 | iner_max.y=max_cloudCPn.y; 445 | outer_min.z=min_cloudCPn.z-Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.z-min_cloudCPn.z)/2; 446 | outer_max.z=max_cloudCPn.z+Config::get("normal_estimate_area_ratio")*std::abs(max_cloudCPn.z-min_cloudCPn.z)/2; 447 | iner_min.z=min_cloudCPn.z; 448 | iner_max.z=max_cloudCPn.z; 449 | pcl::Normal avg_normal_estimation_area = getLoopNormal(cloud_filtered, normals,outer_max,outer_min, iner_max,iner_min); 450 | std::cout<<"avg_normal_estimation_area: " 451 | <::Ptr cloud_filtered_CPn(new pcl::PointCloud); 458 | pcl::PassThrough pass; 459 | pass.setInputCloud (cloud); 460 | pass.setFilterFieldName ("x"); 461 | pass.setFilterLimits (outer_min.x,outer_max.x); 462 | std::cout<<"PASS_filter_x_min: "< normal_estimate_area_cloudCPn (cloud_filtered_CPn, 255, 255, 255); 472 | viewer3->addPointCloud (cloud_filtered_CPn, normal_estimate_area_cloudCPn, cloud_id+"_normal_estimate_area"); 473 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id+"_normal_estimate_area"); 474 | viewer3->spinOnce(100); 475 | getchar(); 476 | 477 | pass.setInputCloud (cloud_filtered_CPn); 478 | pass.setFilterFieldName ("x"); 479 | pass.setFilterLimits (min_cloudCPn.x, max_cloudCPn.x); 480 | std::cout<<"PASS_filter_x_min: "< normal_estimate_area_cloudCPn2 (cloud_filtered_CPn, 0, 255, 255); 492 | viewer3->addPointCloud (cloud_filtered_CPn, normal_estimate_area_cloudCPn2, cloud_id+"_normal_estimate_area2"); 493 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_id+"_normal_estimate_area2"); 494 | viewer3->spinOnce(100); 495 | getchar(); 496 | 497 | } 498 | } 499 | 500 | 501 | 502 | //可视化，法向量检测到的缺陷 503 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP (cloudCP, 255, 0, 0); 504 | // viewer3->addPointCloud (cloudCP, target_color_cloudCP, "cloudCP"); 505 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP"); 506 | 507 | //可视化，曲率检测到的缺陷 508 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP2 (cloudCP2, 0, 0, 255); 509 | // viewer3->addPointCloud (cloudCP2, target_color_cloudCP2, "cloudCP2"); 510 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP2"); 511 | 512 | //可视化，Z阈值检测到的缺陷 513 | // pcl::visualization::PointCloudColorHandlerCustom target_color_cloudCP3 (cloudCP3, 0, 255, 255); 514 | // viewer3->addPointCloud (cloudCP3, target_color_cloudCP3, "cloudCP3"); 515 | // viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloudCP3"); 516 | 517 | while(!viewer3->wasStopped()) 518 | { 519 | viewer3->spinOnce(100); 520 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 521 | } 522 | } 523 | else 524 | { 525 | //创建法线估计的对象 526 | pcl::NormalEstimation normalEstimation; 527 | normalEstimation.setInputCloud(cloud); 528 | //对于每一个点都用半径为3cm的近邻搜索方式 529 | normalEstimation.setRadiusSearch(Config::get("RadiusSearch")); 530 | //Kd_tree是一种数据结构便于管理点云以及搜索点云，法线估计对象会使用这种结构来找到哦啊最近邻点 531 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 532 | normalEstimation.setSearchMethod(kdtree); 533 | //计算法线 534 | normalEstimation.compute(*normals); 535 | 536 | //可视化 537 | boost::shared_ptr viewer3(new pcl::visualization::PCLVisualizer("Cloud Normals")); 538 | viewer3->setBackgroundColor (0, 0, 0); 539 | viewer3->addCoordinateSystem(0.3*(max.y-min.y)); 540 | // Coloring and visualizing target cloud (red). 541 | pcl::visualization::PointCloudColorHandlerCustom target_color (cloud, 255, 0, 0); 542 | viewer3->addPointCloud (cloud, target_color, "cloud"); 543 | viewer3->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); 544 | pcl::visualization::PointCloudColorHandlerCustom normal_color (normals, 255, 255, 255); 545 | viewer3->addPointCloudNormals(cloud, normals, 1, (0.1*(max.x-min.x)), "normals"); 546 | while(!viewer3->wasStopped()) 547 | { 548 | viewer3->spinOnce(100); 549 | boost::this_thread::sleep(boost::posix_time::microseconds(100)); 550 | } 551 | } 552 | return 0; 553 | } -------------------------------------------------------------------------------- /src/normalEstimate_pcl_website.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char **argv) 10 | { 11 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 12 | 13 | if(pcl::io::loadPCDFile(argv[1],*cloud) !=0) 14 | { 15 | return -1; 16 | } 17 | 18 | // Create the normal estimation class, and pass the input dataset to it 19 | pcl::NormalEstimation ne; 20 | ne.setInputCloud (cloud); 21 | 22 | // Create an empty kdtree representation, and pass it to the normal estimation object. 23 | // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). 24 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); 25 | ne.setSearchMethod (tree); 26 | 27 | // Output datasets 28 | pcl::PointCloud::Ptr normals (new pcl::PointCloud); 29 | 30 | // Use all neighbors in a sphere of radius 3cm 31 | ne.setRadiusSearch (0.03); 32 | 33 | // Compute the features 34 | ne.compute (*normals); 35 | 36 | // normals->points.size () should have the same size as the input cloud->points.size ()* 37 | 38 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Cloud Normals")); 39 | viewer->addPointCloud(cloud,"cloud"); 40 | viewer->addPointCloudNormals(cloud, normals, 1, 0.01, "normals"); 41 | 42 | while(!viewer->wasStopped()) 43 | { 44 | viewer->spinOnce(100); 45 | boost::this_thread::sleep(boost::posix_time::microseconds(1000000)); 46 | } 47 | 48 | return 0; 49 | } -------------------------------------------------------------------------------- /src/python cv2.txt~: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZiqiChai/3dDefectDetection/36b1b359a320e2fa234d2095f2069c33f4fa8db6/src/python cv2.txt~ -------------------------------------------------------------------------------- /src/tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) 2 | 3 | 4 | ######################################## TOOLS ############################################## 5 | 6 | add_executable(ply2pcd ply2pcd.cpp ) 7 | target_link_libraries (ply2pcd ${PCL_LIBRARIES}) 8 | 9 | add_executable(pcd2ply pcd2ply.cpp ) 10 | target_link_libraries (pcd2ply ${PCL_LIBRARIES}) 11 | 12 | ######################################## install ############################################## 13 | INSTALL(TARGETS ply2pcd pcd2ply 14 | RUNTIME DESTINATION bin 15 | LIBRARY DESTINATION lib 16 | ARCHIVE DESTINATION libstatic 17 | ) 18 | 19 | #other files 20 | #INSTALL(FILES files... DESTINATION 21 | #[PERMISSIONS permissions...] 22 | #[CONFIGURATIONS [Debug|Release|...]] 23 | #[COMPONENT ] 24 | #[RENAME ] [OPTIONAL]) 25 | 26 | #other script 27 | #INSTALL(PROGRAMS files... DESTINATION 28 | #[PERMISSIONS permissions...] 29 | #[CONFIGURATIONS [Debug|Release|...]] 30 | #[COMPONENT ] 31 | #[RENAME ] [OPTIONAL]) 32 | 33 | #dirs 34 | #INSTALL(DIRECTORY icons scripts/ DESTINATION share/myproj 35 | #PATTERN "CVS" EXCLUDE 36 | #PATTERN "scripts/*" 37 | #PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READGROUP_EXECUTE GROUP_READ) -------------------------------------------------------------------------------- /src/tools/pcd2ply.cpp: -------------------------------------------------------------------------------- 1 | //load PCD and save PLY Data 2 | //C++ 3 | #include 4 | //PCL 5 | #include 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) 10 | { 11 | //创建点云对象 12 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 13 | 14 | //读取PCD文件 15 | if(pcl::io::loadPCDFile(argv[1], *cloud) !=0) 16 | { 17 | PCL_ERROR("Couldn't read PCD file \n"); 18 | return -1; 19 | } 20 | std::cout << "Loaded " << cloud->width * cloud->height << " data points from PCD file " << std::endl; 21 | // for (size_t i = 0; i < cloud->points.size(); ++i) 22 | // std::cout << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << "\r"; 23 | pcl::io::savePLYFileASCII(argv[2], *cloud); 24 | return (0); 25 | } -------------------------------------------------------------------------------- /src/tools/ply2pcd.cpp: -------------------------------------------------------------------------------- 1 | //load PLY and save PCD Data 2 | //C++ 3 | #include 4 | //PCL 5 | #include 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) 10 | { 11 | //创建点云对象 12 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 13 | 14 | //读取PLY文件 15 | if (pcl::io::loadPLYFile(argv[1],*cloud) !=0) 16 | { 17 | PCL_ERROR("Couldn't read PLY file \n"); 18 | return (-1); 19 | } 20 | std::cout << "Loaded " << cloud->width * cloud->height << " data points from PLY file " << std::endl; 21 | // for (size_t i = 0; i < cloud->points.size(); ++i) 22 | // std::cout << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << "\r"; 23 | pcl::io::savePCDFileASCII(argv[2],*cloud); 24 | return (0); 25 | } --------------------------------------------------------------------------------