├── 3dKDtree.cpp ├── 3dKDtree.h ├── CMakeLists.txt ├── CMakeLists.txt.user ├── Matrix.cpp ├── Matrix.h ├── Model.cpp ├── Model.h ├── Plane.cpp ├── Plane.h ├── PlaneFitting.cpp ├── PlaneFitting.h ├── Point.cpp ├── Point.h ├── PointIO.cpp ├── PointIO.h ├── README.md ├── Segmentation.cpp ├── Segmentation.h ├── kdtree.cpp ├── kdtree.h └── main.cpp /3dKDtree.cpp: -------------------------------------------------------------------------------- 1 | #include "3dKDtree.h" 2 | 3 | kdTree::kdTree(const int nodes) 4 | { 5 | storedKDNodes = 0; 6 | maxNumOfNodes = nodes; 7 | 8 | kdNodes = new kdNode[maxNumOfNodes + 1]; 9 | if(!kdNodes) 10 | { 11 | std::cout<<"初始化kd树时内存溢出！"< 1) 27 | { 28 | kdNode **pa1 = new kdNode*[storedKDNodes + 1]; //组织好树后的指针 29 | kdNode **pa2 = new kdNode*[storedKDNodes + 1]; //原始元素的指针 30 | 31 | for(int i =0; i <= storedKDNodes; i++) 32 | pa2[i] = &kdNodes[i]; 33 | 34 | balancePartition(pa1, pa2, 1, 1, storedKDNodes); 35 | delete []pa2; 36 | 37 | //重新排列树 38 | //__w64 int d, j = 1; // According to the warning given when 'int ' is used 39 | int d, j = 1; //j位置元素已经转移走 40 | int foo = 1; //fooNodes存储的元素的最初位置 41 | kdNode fooNodes = kdNodes[j]; 42 | 43 | for(int i = 1; i <= storedKDNodes; i++) 44 | { 45 | d = pa1[j] - kdNodes; 46 | pa1[j] = NULL; 47 | 48 | if(d != foo) 49 | kdNodes[j] = kdNodes[d]; 50 | else 51 | { 52 | kdNodes[j] = fooNodes; 53 | 54 | if(i < storedKDNodes) 55 | { 56 | for(; foo <= storedKDNodes; foo++) 57 | if(NULL != pa1[foo]) 58 | break; 59 | 60 | fooNodes = kdNodes[foo]; 61 | j = foo; 62 | } 63 | continue; 64 | } 65 | j = d; 66 | } 67 | delete []pa1; 68 | } 69 | halfStoredKDNodes = storedKDNodes/2 - 1; 70 | } 71 | 72 | void kdTree::locateNodes(nNearestNodes * const nNN,const int index)const 73 | { 74 | const kdNode *p = &kdNodes[index]; 75 | double dist1; 76 | 77 | if(index < halfStoredKDNodes) 78 | { 79 | dist1 = nNN->pos[p->plane] - p->pos[p->plane]; 80 | 81 | if(0.0 < dist1) 82 | { 83 | locateNodes(nNN, 2 * index + 1); 84 | if(nNN->dist2[0] > dist1 * dist1) 85 | locateNodes(nNN, 2 * index); 86 | } 87 | else 88 | { 89 | locateNodes(nNN, 2 * index); 90 | if(nNN->dist2[0] > dist1 * dist1) 91 | locateNodes(nNN, 2 * index + 1); 92 | }//if 93 | }//if 94 | 95 | // 计算距离 96 | dist1 = p->pos[0] - nNN->pos[0]; 97 | double dist2 = dist1 * dist1; 98 | dist1 = p->pos[1] - nNN->pos[1]; 99 | dist2 += dist1 * dist1; 100 | dist1 = p->pos[2] - nNN->pos[2]; 101 | dist2 += dist1 * dist1; 102 | 103 | if(nNN->dist2[0] > dist2) 104 | { 105 | if(nNN->found < nNN->max) 106 | { 107 | nNN->found++; 108 | nNN->dist2[nNN->found] = dist2; 109 | nNN->index[nNN->found] = p; 110 | } 111 | else 112 | { 113 | int j, parent; 114 | if(0 == nNN->got_Heap)//建立大顶堆 115 | { 116 | double dst2; 117 | const kdNode *nd; 118 | int halfFound = nNN->found >> 1; 119 | 120 | for(int k = halfFound; k >= 1; k--) 121 | { 122 | parent = k; 123 | nd = nNN->index[k]; 124 | dst2 = nNN->dist2[k]; 125 | 126 | while(parent <= halfFound) 127 | { 128 | j = parent + parent; 129 | 130 | if(j < nNN->found && nNN->dist2[j] < nNN->dist2[j + 1]) 131 | j ++; 132 | 133 | if(dst2 >= nNN->dist2[j]) 134 | break; 135 | 136 | nNN->dist2[parent] = nNN->dist2[j]; 137 | nNN->index[parent] = nNN->index[j]; 138 | 139 | parent = j; 140 | }//while 141 | nNN->dist2[parent] = dst2; 142 | nNN->index[parent] = nd; 143 | }//for 144 | nNN->got_Heap = 1; 145 | }//if 146 | 147 | //插入 148 | parent = 1; 149 | //if() 150 | j = 2; 151 | 152 | while(j <= nNN->found) 153 | { 154 | if(j < nNN->found && nNN->dist2[j] < nNN->dist2[j + 1]) 155 | j++; 156 | 157 | if(dist2 > nNN->dist2[j]) 158 | break; 159 | 160 | nNN->dist2[parent] = nNN->dist2[j]; 161 | nNN->index[parent] = nNN->index[j]; 162 | parent = j; 163 | j += j; 164 | }//while 165 | 166 | if((parent != 1)||(dist2 < nNN->dist2[parent])) 167 | { 168 | nNN->index[parent] = p; 169 | nNN->dist2[parent] = dist2; 170 | } 171 | nNN->dist2[0] = nNN->dist2[1];//?????? 172 | }//else 173 | }//if 174 | } 175 | 176 | #define swap(kdN,a,b){ kdNode* tmp = kdN[a]; kdN[a] = kdN[b]; kdN[b] = tmp;} 177 | 178 | void kdTree::medianPartition(kdNode** pOrig,const int start,const int end,const int median,const int axis) 179 | { 180 | int left = start; 181 | int right = end; 182 | 183 | while(right > left) 184 | { 185 | const TYPE v = pOrig[right]->pos[axis]; 186 | 187 | int i = left - 1; 188 | int j = right; 189 | 190 | for(;;) 191 | { 192 | while(pOrig[++i]->pos[axis] < v); 193 | while(pOrig[--j]->pos[axis] > v && j > left); 194 | if(i >= j) 195 | break; 196 | 197 | swap(pOrig, i, j); 198 | } 199 | 200 | swap(pOrig, i, right); 201 | if(i >= median) 202 | right = i - 1; 203 | if(i <= median) 204 | left = i + 1; 205 | } 206 | } 207 | void kdTree::balancePartition(kdNode** pBalanced,kdNode** pOriginal,const int index,const int start,const int end) 208 | { 209 | //计算median，这是怎么计算的呢？？？ 210 | int median = 1; 211 | while((4 * median) <= (end - start + 1)) 212 | median += median; //median*=2; 213 | 214 | if((3 * median) <= (end - start +1)) 215 | { 216 | median += median; 217 | median += start - 1; 218 | } 219 | else 220 | median = end - median + 1; 221 | 222 | // 寻找分割数据的轴 223 | int axis = 2; 224 | if((boundrayMax[0] - boundrayMin[0]) > (boundrayMax[1] - boundrayMin[1])&& 225 | (boundrayMax[0] - boundrayMin[0]) > (boundrayMax[2] - boundrayMin[2])) 226 | axis = 0; 227 | else if((boundrayMax[1] - boundrayMin[1]) > (boundrayMax[2] - boundrayMin[2])) 228 | axis = 1; 229 | 230 | // 按median分割节点 231 | medianPartition(pOriginal, start, end, median, axis); 232 | 233 | pBalanced[index] = pOriginal[median]; 234 | pBalanced[index]->plane = axis; 235 | 236 | // 迭代平衡左右子树 237 | if(median > start) 238 | { 239 | if(start < median - 1) 240 | { 241 | const double tmp = boundrayMax[axis]; 242 | boundrayMax[axis] = pBalanced[index]->pos[axis]; 243 | balancePartition(pBalanced, pOriginal, 2 * index, start, median - 1); 244 | boundrayMax[axis] = tmp; 245 | } 246 | else 247 | pBalanced[2 * index] = pOriginal[start]; 248 | } 249 | if(median < end) 250 | { 251 | if(median + 1 < end) 252 | { 253 | const double tmp = boundrayMin[axis]; 254 | boundrayMin[axis] = pBalanced[index]->pos[axis]; 255 | balancePartition(pBalanced, pOriginal, 2 * index + 1, median + 1, end); 256 | boundrayMin[axis] = tmp; 257 | } 258 | else 259 | pBalanced[2 * index + 1] = pOriginal[end]; 260 | } 261 | } -------------------------------------------------------------------------------- /3dKDtree.h: -------------------------------------------------------------------------------- 1 | #ifndef _3DKDTREE_H_ 2 | #define _3DKDTREE_H_ 3 | 4 | #define TYPE double 5 | 6 | #include 7 | 8 | 9 | //------------------------------- 10 | // KD树节点 11 | //------------------------------- 12 | typedef struct kdNode 13 | { 14 | TYPE pos[3]; //节点坐标 15 | short plane; //节点的分割平面 16 | int id; //从UCD文件格式读取的节点的ID 17 | }kdNode; 18 | 19 | //------------------------------------------- 20 | // KD树 21 | //------------------------------------------- 22 | class nNearestNodes; 23 | class kdTree 24 | { 25 | public: 26 | // 27 | // 构造函数 28 | // nodes:树的最大节点数目，用来分配内存空间 29 | // 30 | kdTree(const int nodes); 31 | kdTree(){} //无参构造函数 32 | // 33 | // 析构函数 34 | // 35 | ~kdTree(); 36 | // 37 | // 在树中存储一个节点,按读取顺序存储的 38 | // 39 | inline void store(const TYPE xCoord,const TYPE yCoord,const TYPE zCoord,const int pId); 40 | inline void store(const TYPE coords[3],const int pId); 41 | 42 | // 43 | // 平衡树 44 | // 45 | void treeBalance(); 46 | 47 | // 48 | // 在index开始的子树中寻找邻近点 49 | // 50 | void locateNodes(nNearestNodes * const nNN,const int index)const; 51 | 52 | // 53 | // 54 | // 55 | void balancePartition(kdNode** pBalanced,kdNode** pOriginal,const int index,const int start,const int end); 56 | 57 | // 58 | // 59 | // 60 | void medianPartition(kdNode** pOrig,const int start,const int end,const int median,const int axis); 61 | protected: 62 | kdNode* kdNodes; 63 | 64 | int storedKDNodes; 65 | int halfStoredKDNodes; //非叶子节点数 66 | int maxNumOfNodes; 67 | 68 | double boundrayMin[3]; //三个方向的最小值 69 | double boundrayMax[3]; //三个方向的最大值，这两个值相当于所有三维点的包围框 70 | }; 71 | 72 | //---------------------------------------- 73 | // 最近临节点 74 | //---------------------------------------- 75 | class nNearestNodes 76 | { 77 | public: 78 | int max; //最近临节点的数目 79 | int found; //搜索过程中找到的nNN的数目 80 | int got_Heap; //寻找nNN时是否已经建堆 0表示没有建堆 81 | double pos[3]; //指定点的坐标 82 | double* dist2; //找到最近临点到指定点的距离的平方 83 | const kdNode** index; //找到的最近临点 84 | 85 | // 86 | // 构造函数 87 | // 88 | nNearestNodes(const int maximum) 89 | { 90 | max = maximum; 91 | found = got_Heap = 0; 92 | } 93 | // 94 | // 析构函数 95 | // 96 | ~nNearestNodes() 97 | { 98 | delete[]dist2; 99 | delete[]index; 100 | } 101 | 102 | //dis ：搜索距离的阈值 103 | void setDistandIndx(double dis = 1.0) 104 | { 105 | dist2 = new double[max + 1]; 106 | dist2[0] = dis * dis; //最小搜索距离的平方,小于这个距离的点认为是要搜索的 107 | //dist2[0] = dis; 108 | index = new const kdNode*[max + 1]; 109 | } 110 | 111 | void setSearchPnt(const double x,const double y,const double z) 112 | { 113 | pos[0] = x; 114 | pos[1] = y; 115 | pos[2] = z; 116 | } 117 | }; 118 | 119 | //----------------------------------------------------------------- 120 | void kdTree::store(const TYPE xCoord,const TYPE yCoord,const TYPE zCoord,const int pId) 121 | { 122 | const TYPE coords[3] = {xCoord,yCoord,zCoord}; 123 | store(coords,pId); 124 | } 125 | 126 | void kdTree::store(const TYPE coords[3],const int pId) 127 | { 128 | if(storedKDNodes > maxNumOfNodes) 129 | { 130 | std::cout << "No room for more nodes..." << std::endl; 131 | return; 132 | } 133 | 134 | storedKDNodes++; 135 | kdNode *const node = &kdNodes[storedKDNodes]; 136 | for(int i = 0; i < 3; i++) 137 | { 138 | node->pos[i] = coords[i]; 139 | 140 | if(node->pos[i] < boundrayMin[i]) 141 | boundrayMin[i] = node->pos[i]; 142 | if(node->pos[i] > boundrayMax[i]) 143 | boundrayMax[i] = node->pos[i]; 144 | } 145 | node->id = pId; 146 | } 147 | 148 | #endif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | project(plane_segmentation) 4 | 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | find_package(PCL 1.2 REQUIRED) 8 | 9 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 10 | 11 | include_directories(${PCL_INCLUDE_DIRS}) 12 | link_directories(${PCL_LIBRARY_DIRS}) 13 | add_definitions(${PCL_DEFINITIONS}) 14 | 15 | 16 | add_executable (plane_segmentation main.cpp Model.cpp Plane.cpp Point.cpp kdtree.cpp Matrix.cpp PointIO.cpp 3dKDtree.cpp PlaneFitting.cpp Segmentation.cpp) 17 | target_link_libraries (plane_segmentation ${PCL_LIBRARIES}) 18 | 19 | 20 | -------------------------------------------------------------------------------- /CMakeLists.txt.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | EnvironmentId 7 | {cf8e757f-645a-4b87-af17-f0a6586d3edc} 8 | 9 | 10 | ProjectExplorer.Project.ActiveTarget 11 | 0 12 | 13 | 14 | ProjectExplorer.Project.EditorSettings 15 | 16 | true 17 | false 18 | true 19 | 20 | Cpp 21 | 22 | CppGlobal 23 | 24 | 25 | 26 | QmlJS 27 | 28 | QmlJSGlobal 29 | 30 | 31 | 2 32 | UTF-8 33 | false 34 | 4 35 | false 36 | 80 37 | true 38 | true 39 | 1 40 | true 41 | false 42 | 0 43 | true 44 | true 45 | 0 46 | 8 47 | true 48 | 1 49 | true 50 | true 51 | true 52 | false 53 | 54 | 55 | 56 | ProjectExplorer.Project.PluginSettings 57 | 58 | 59 | 60 | ProjectExplorer.Project.Target.0 61 | 62 | Desktop Qt 5.6.2 GCC 64bit 63 | Desktop Qt 5.6.2 GCC 64bit 64 | qt.56.gcc_64_kit 65 | 0 66 | 0 67 | 0 68 | 69 | 70 | /home/zfluo/桌面/segmentation/code/src/build-src-Default 71 | 72 | 73 | 74 | 75 | all 76 | 77 | true 78 | Make 79 | 80 | CMakeProjectManager.MakeStep 81 | 82 | 1 83 | 构建 84 | 85 | ProjectExplorer.BuildSteps.Build 86 | 87 | 88 | 89 | 90 | 91 | clean 92 | 93 | true 94 | Make 95 | 96 | CMakeProjectManager.MakeStep 97 | 98 | 1 99 | 清理 100 | 101 | ProjectExplorer.BuildSteps.Clean 102 | 103 | 2 104 | false 105 | 106 | Default 107 | Default 108 | CMakeProjectManager.CMakeBuildConfiguration 109 | 110 | 111 | 112 | CMAKE_BUILD_TYPE:STRING=Debug 113 | 114 | /home/zfluo/桌面/segmentation/code/src/build-src-Debug 115 | 116 | 117 | 118 | 119 | 120 | 121 | true 122 | Make 123 | 124 | CMakeProjectManager.MakeStep 125 | 126 | 1 127 | 构建 128 | 129 | ProjectExplorer.BuildSteps.Build 130 | 131 | 132 | 133 | 134 | 135 | clean 136 | 137 | true 138 | Make 139 | 140 | CMakeProjectManager.MakeStep 141 | 142 | 1 143 | 清理 144 | 145 | ProjectExplorer.BuildSteps.Clean 146 | 147 | 2 148 | false 149 | 150 | Debug 151 | Debug 152 | CMakeProjectManager.CMakeBuildConfiguration 153 | 154 | 155 | 156 | CMAKE_BUILD_TYPE:STRING=Release 157 | 158 | /home/zfluo/桌面/segmentation/code/src/build-src-Release 159 | 160 | 161 | 162 | 163 | 164 | 165 | true 166 | Make 167 | 168 | CMakeProjectManager.MakeStep 169 | 170 | 1 171 | 构建 172 | 173 | ProjectExplorer.BuildSteps.Build 174 | 175 | 176 | 177 | 178 | 179 | clean 180 | 181 | true 182 | Make 183 | 184 | CMakeProjectManager.MakeStep 185 | 186 | 1 187 | 清理 188 | 189 | ProjectExplorer.BuildSteps.Clean 190 | 191 | 2 192 | false 193 | 194 | Release 195 | Release 196 | CMakeProjectManager.CMakeBuildConfiguration 197 | 198 | 199 | 200 | CMAKE_BUILD_TYPE:STRING=RelWithDebInfo 201 | 202 | /home/zfluo/桌面/segmentation/code/src/build-src-Release with Debug Information 203 | 204 | 205 | 206 | 207 | 208 | 209 | true 210 | Make 211 | 212 | CMakeProjectManager.MakeStep 213 | 214 | 1 215 | 构建 216 | 217 | ProjectExplorer.BuildSteps.Build 218 | 219 | 220 | 221 | 222 | 223 | clean 224 | 225 | true 226 | Make 227 | 228 | CMakeProjectManager.MakeStep 229 | 230 | 1 231 | 清理 232 | 233 | ProjectExplorer.BuildSteps.Clean 234 | 235 | 2 236 | false 237 | 238 | Release with Debug Information 239 | Release with Debug Information 240 | CMakeProjectManager.CMakeBuildConfiguration 241 | 242 | 243 | 244 | CMAKE_BUILD_TYPE:STRING=MinSizeRel 245 | 246 | /home/zfluo/桌面/segmentation/code/src/build-src-Minimum Size Release 247 | 248 | 249 | 250 | 251 | 252 | 253 | true 254 | Make 255 | 256 | CMakeProjectManager.MakeStep 257 | 258 | 1 259 | 构建 260 | 261 | ProjectExplorer.BuildSteps.Build 262 | 263 | 264 | 265 | 266 | 267 | clean 268 | 269 | true 270 | Make 271 | 272 | CMakeProjectManager.MakeStep 273 | 274 | 1 275 | 清理 276 | 277 | ProjectExplorer.BuildSteps.Clean 278 | 279 | 2 280 | false 281 | 282 | Minimum Size Release 283 | Minimum Size Release 284 | CMakeProjectManager.CMakeBuildConfiguration 285 | 286 | 5 287 | 288 | 289 | 0 290 | 部署 291 | 292 | ProjectExplorer.BuildSteps.Deploy 293 | 294 | 1 295 | 在本地部署 296 | 297 | ProjectExplorer.DefaultDeployConfiguration 298 | 299 | 1 300 | 301 | 302 | false 303 | false 304 | 1000 305 | 306 | true 307 | 308 | false 309 | false 310 | false 311 | false 312 | true 313 | 0.01 314 | 10 315 | true 316 | 1 317 | 25 318 | 319 | 1 320 | true 321 | false 322 | true 323 | valgrind 324 | 325 | 0 326 | 1 327 | 2 328 | 3 329 | 4 330 | 5 331 | 6 332 | 7 333 | 8 334 | 9 335 | 10 336 | 11 337 | 12 338 | 13 339 | 14 340 | 341 | plane_segmentation 342 | 343 | 344 | /home/zfluo/桌面/segmentation/code/src/build-src-Default 345 | 2 346 | 347 | plane_segmentation 348 | 349 | CMakeProjectManager.CMakeRunConfiguration.plane_segmentation 350 | 3768 351 | false 352 | true 353 | false 354 | false 355 | true 356 | 357 | 1 358 | 359 | 360 | 361 | ProjectExplorer.Project.TargetCount 362 | 1 363 | 364 | 365 | ProjectExplorer.Project.Updater.FileVersion 366 | 18 367 | 368 | 369 | Version 370 | 18 371 | 372 | 373 | -------------------------------------------------------------------------------- /Matrix.cpp: -------------------------------------------------------------------------------- 1 | #include "Matrix.h" 2 | 3 | 4 | td::Matrix::Matrix() 5 | { 6 | } 7 | 8 | 9 | td::Matrix::~Matrix() 10 | { 11 | } 12 | -------------------------------------------------------------------------------- /Matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | *计算矩阵最小特征值对应特征向量 3 | *罗中飞，2015.12 4 | */ 5 | 6 | #ifndef matrix_h 7 | #define matrix_h 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace td 14 | { 15 | class Matrix 16 | { 17 | public: 18 | Matrix(); 19 | ~Matrix(); 20 | 21 | //计算最小特征值对应的特征向量 22 | static void minEigenvalueVector(Eigen::Matrix3d &_mat, Eigen::MatrixXd &_vectors) 23 | { 24 | Eigen::EigenSolver es(_mat); 25 | Eigen::Matrix3d D = es.pseudoEigenvalueMatrix(); 26 | Eigen::Matrix3d V = es.pseudoEigenvectors(); 27 | int id = 0; 28 | double minv = LONG_MAX; 29 | for (int i = 0; i < _mat.rows(); ++i) 30 | { 31 | if (abs(D(i, i)) < minv) 32 | { 33 | minv = abs(D(i, i)); 34 | id = i; 35 | } 36 | } 37 | _vectors << V(0, id), V(1, id), V(2, id); 38 | } 39 | }; 40 | } 41 | 42 | #endif 43 | 44 | -------------------------------------------------------------------------------- /Model.cpp: -------------------------------------------------------------------------------- 1 | #include "Model.h" 2 | 3 | 4 | td::Model::Model() 5 | : needNum(0) 6 | { 7 | } 8 | 9 | 10 | td::Model::~Model() 11 | { 12 | } 13 | -------------------------------------------------------------------------------- /Model.h: -------------------------------------------------------------------------------- 1 | /* 2 | *模型基类 3 | *罗中飞，2015.12 4 | */ 5 | #ifndef model_h 6 | #define model_h 7 | 8 | namespace td 9 | { 10 | class Model 11 | { 12 | public: 13 | Model(); 14 | ~Model(); 15 | 16 | // 模型拟合需要的点数 17 | int needNum; 18 | }; 19 | } 20 | #endif 21 | 22 | -------------------------------------------------------------------------------- /Plane.cpp: -------------------------------------------------------------------------------- 1 | #include "Plane.h" 2 | 3 | 4 | td::Plane::Plane(): 5 | a(0), 6 | b(0), 7 | c(0), 8 | d(0) 9 | { 10 | needNum = 3; 11 | } 12 | 13 | td::Plane::Plane(double a,double b,double c,double d) 14 | { 15 | this->a = a; 16 | this->b = b; 17 | this->c = c; 18 | this->d = d; 19 | needNum = 3; 20 | } 21 | 22 | td::Plane::~Plane() 23 | { 24 | } 25 | 26 | //通过点集计算参数 27 | void td::Plane::computeFromPoints(PointCloud& cloud) 28 | { 29 | PointCloud::iterator temIt; 30 | 31 | Eigen::Matrix3d A; 32 | 33 | for (int i = 0; i < 3; ++i) 34 | { 35 | for (int j = 0; j < 3; ++j) 36 | { 37 | A(i, j) = 0; 38 | } 39 | } 40 | 41 | double meanx, meany, meanz, sumx, sumy, sumz; //x,y,z的均值 42 | double detax, detay, detaz; 43 | 44 | sumx = sumy = sumz = 0; 45 | for (temIt = cloud.begin(); temIt != cloud.end(); ++temIt) 46 | { 47 | sumx += temIt->x; 48 | sumy += temIt->y; 49 | sumz += temIt->z; 50 | } 51 | meanx = sumx / cloud.size(); 52 | meany = sumy / cloud.size(); 53 | meanz = sumz / cloud.size(); 54 | 55 | 56 | for (temIt = cloud.begin(); temIt != cloud.end(); ++temIt) 57 | { 58 | detax = temIt->x - meanx; //detax[i]; 59 | detay = temIt->y - meany; //detay[i]; 60 | detaz = temIt->z - meanz; //detaz[i]; 61 | A(0, 0) += detax*detax; //deta_xi^2; 62 | A(0, 1) += detax*detay; //deta_xi*deta_yi; 63 | A(0, 2) += detax*detaz; //deta_xi*deta_zi; 64 | 65 | A(1, 0) += detax*detay; //deta_xi*deta_yi; 66 | A(1, 1) += detay*detay; //deta_yi^2; 67 | A(1, 2) += detay*detaz; //deta_yi*deta_zi; 68 | 69 | A(2, 0) += detax*detaz; //deta_xi*deta_zi; 70 | A(2, 1) += detay*detaz; //deta_yi*deta_zi; 71 | A(2, 2) += detaz*detaz; //deta_zi^2; 72 | } 73 | 74 | Eigen::MatrixXd V(1, 3); 75 | Matrix::minEigenvalueVector(A, V); 76 | 77 | a = V(0, 0); 78 | b = V(0, 1); 79 | c = V(0, 2); 80 | d = a * meanx + b * meany + c * meanz; 81 | } 82 | 83 | //点到平面的距离 84 | double td::Plane::point2plane(Point& pt) 85 | { 86 | double distance(0); 87 | distance = fabs(pt.x*a + pt.y*b + pt.z*c - d); 88 | return distance; 89 | } 90 | 91 | //设置平面参数 92 | void td::Plane::setPara(double a, double b, double c, double d) 93 | { 94 | this->a = a; 95 | this->b = b; 96 | this->c = c; 97 | this->d = d; 98 | } -------------------------------------------------------------------------------- /Plane.h: -------------------------------------------------------------------------------- 1 | /* 2 | *平面模型 3 | *平面方程为：ax + by + cz = d, (d > 0); 4 | *a^2 + b^2 + c^2 = 1 5 | *罗中飞，2015.12 6 | */ 7 | 8 | #ifndef plane_h 9 | #define plane_h 10 | 11 | #include "Model.h" 12 | #include "Point.h" 13 | #include "Matrix.h" 14 | 15 | #include 16 | #include 17 | 18 | namespace td 19 | { 20 | class Plane:public Model 21 | { 22 | public: 23 | Plane(); 24 | Plane(double, double, double, double); 25 | ~Plane(); 26 | 27 | //设置平面参数 28 | void setPara(double, double, double, double); 29 | 30 | //获取平面参数A 31 | double getA() 32 | { 33 | return a; 34 | } 35 | 36 | //获取平面参数B 37 | double getB() 38 | { 39 | return b; 40 | } 41 | 42 | //获取平面参数C 43 | double getC() 44 | { 45 | return c; 46 | } 47 | 48 | //获取平面参数D 49 | double getD() 50 | { 51 | return d; 52 | } 53 | 54 | //通过点集计算参数，方法为PCA 55 | void computeFromPoints(PointCloud& ); 56 | 57 | //点到平面的距离 58 | double point2plane(Point&); 59 | 60 | private: 61 | double a; 62 | double b; 63 | double c; 64 | double d; 65 | }; 66 | } 67 | #endif; 68 | -------------------------------------------------------------------------------- /PlaneFitting.cpp: -------------------------------------------------------------------------------- 1 | #include "PlaneFitting.h" 2 | 3 | 4 | td::PlaneFitting::PlaneFitting() 5 | : inliers_threshold(0) 6 | { 7 | 8 | } 9 | 10 | td::PlaneFitting::PlaneFitting(const td::PointCloud& pc) 11 | { 12 | cloud = pc; 13 | } 14 | 15 | td::PlaneFitting::~PlaneFitting() 16 | { 17 | 18 | } 19 | 20 | //输入点云 21 | void td::PlaneFitting::setInputCloud(const td::PointCloud& pc) 22 | { 23 | cloud = pc; 24 | } 25 | 26 | 27 | //RANSAC进行拟合 28 | bool td::PlaneFitting::computeByRANSAC(double threshold) 29 | { 30 | srand((unsigned)time(NULL)); 31 | if (cloud.size() <= 3) 32 | return false; 33 | //初始迭代次数 34 | int initial_iter = int(ceil(log10(0.01) / log10(1.0 - (double)model.needNum / cloud.size()))); 35 | int iter(0); //RANSAC循环变量 36 | int inliers_l(0), inliers_n(0);//上一次循环局内点数目，本次循环局内点数目 37 | 38 | int randnum = model.needNum; 39 | int* nIndex=new int [randnum]; //随机点索引 40 | 41 | while (iter < initial_iter) 42 | { 43 | 44 | //抽取随机点 45 | for (int i = 0; i < randnum; i++) 46 | { 47 | nIndex[i] = rand() % cloud.size(); 48 | } 49 | 50 | //判断抽取随机点是否重复 51 | bool allsame(true); 52 | for (int i = 1; i < randnum; i++) 53 | { 54 | allsame = allsame&&nIndex[i] == nIndex[i - 1]; 55 | } 56 | if (allsame) 57 | continue; 58 | 59 | Plane pl; 60 | PointCloud randcloud; 61 | for (int i = 0; i < randnum; i++) 62 | { 63 | randcloud.push_back(cloud[nIndex[i]]); 64 | } 65 | pl.computeFromPoints(randcloud); 66 | 67 | double d2plane(0); 68 | inliers_n = 0; 69 | for (size_t i = 0; i < cloud.size(); ++i) 70 | { 71 | d2plane = pl.point2plane(cloud[i]); 72 | if (d2plane <= threshold) 73 | { 74 | inliers_n++; 75 | } 76 | } 77 | 78 | 79 | if (inliers_n > inliers_l) 80 | { 81 | inliers_l = inliers_n; 82 | initial_iter = int(ceil(log10(0.01) / log10(1.0 - pow((double)inliers_n / cloud.size(), 3))));//更新循环次数 83 | model.setPara(pl.getA(), pl.getB(), pl.getC(), pl.getD()); 84 | iter = 0; 85 | continue;//进行下一次循环 86 | } 87 | iter++; 88 | } 89 | delete[]nIndex; 90 | inliers_threshold = threshold; 91 | return true; 92 | 93 | } 94 | 95 | //BAYSAC进行拟合 96 | bool td::PlaneFitting::computeByBAYSAC(double threshold) 97 | { 98 | srand((unsigned)time(NULL)); 99 | if (cloud.size() <= 3) 100 | return false; 101 | 102 | std::vector pro(cloud.size(), 0);//概率值，索引与点一一对应 103 | 104 | //初始迭代次数 105 | int initial_iter = int(ceil(log10(0.01) / log10(1.0 - (double)model.needNum / cloud.size()))); 106 | int stas_num = int(0.1*(double)initial_iter); 107 | 108 | 109 | int randnum = model.needNum; 110 | int* nIndex = new int[randnum]; //随机点索引 111 | 112 | 113 | double para_percent = 0;//最优参数占总参数的比例 114 | 115 | //用于参数统计 116 | std::vector ins, plnum; //记录平面参数 117 | std::vector planes; 118 | 119 | //参数统计 120 | for (int iter = 0; iter < 6; ) 121 | { 122 | 123 | //抽取随机点 124 | for (int i = 0; i < randnum; i++) 125 | { 126 | nIndex[i] = rand() % cloud.size(); 127 | } 128 | 129 | //判断抽取随机点是否重复 130 | bool allsame(true); 131 | for (int i = 1; i < randnum; i++) 132 | { 133 | allsame = allsame&&nIndex[i] == nIndex[i - 1]; 134 | } 135 | if (allsame) 136 | continue; 137 | 138 | Plane pl; 139 | PointCloud randcloud; 140 | for (int i = 0; i < randnum; i++) 141 | { 142 | randcloud.push_back(cloud[nIndex[i]]); 143 | } 144 | pl.computeFromPoints(randcloud); 145 | 146 | double d2plane(0); 147 | int inliers_n = 0; 148 | for (size_t i = 0; i < cloud.size(); ++i) 149 | { 150 | d2plane = pl.point2plane(cloud[i]); 151 | if (d2plane <= threshold) 152 | { 153 | inliers_n++; 154 | } 155 | } 156 | 157 | iter++; 158 | 159 | //每次计算的参数放入统计序列中 160 | planes.push_back(pl); 161 | ins.push_back(inliers_n);//该参数下局内点的个数 162 | plnum.push_back(1); 163 | 164 | 165 | //最优参数统计,当前参数和前面的参数进行比较, 166 | if (iter >= 1) 167 | { 168 | for (size_t i = 0; i < planes.size() - 1; ++i) 169 | { 170 | double para_dif = acos(pl.getA() * planes[i].getA() + pl.getB() * planes[i].getB() 171 | + pl.getC() * planes[i].getC()) * 180 /3.14; 172 | para_dif = para_dif > 90 ? 180 - para_dif : para_dif; 173 | if (para_dif < 10)/*点到精度高角度小，点的精度差角度大*/ 174 | { 175 | plnum[i]++;//统计第i套参数的数目 176 | if (inliers_n>ins[i]) 177 | { 178 | planes[i].setPara(pl.getA(), pl.getB(), pl.getC(), pl.getD()); 179 | ins[i] = inliers_n; 180 | } 181 | planes.pop_back(); 182 | ins.pop_back(); 183 | plnum.pop_back(); 184 | } 185 | } 186 | para_percent = *max_element(plnum.begin(), plnum.end()) / (double)(iter); 187 | } 188 | } 189 | //贝叶斯过程 190 | std::vector::iterator it_f = plnum.begin(); 191 | std::vector::iterator it_s = max_element(plnum.begin(), plnum.end()); 192 | size_t para_best_index = it_s - it_f; 193 | 194 | //通过参数统计为点赋先验概率 195 | for (size_t i = 0; i < cloud.size(); i++) 196 | { 197 | double d2plane = planes[para_best_index].point2plane(cloud[i]); 198 | if (d2plane <= threshold) 199 | { 200 | pro[i] = 1 - d2plane / threshold; 201 | } 202 | } 203 | 204 | //重新开始循环，参数统计步骤已经结束 205 | int iter = 0; 206 | int inliers_l = 0; 207 | int inliers_n = 0; 208 | 209 | while (iter < initial_iter) 210 | { 211 | //找出概率值最高的三个点 212 | std::vector temp = pro; 213 | for (int i = 0; i < randnum; i++) 214 | { 215 | std::vector::iterator it; 216 | it = max_element(temp.begin(), temp.end()); 217 | nIndex[i] = it - temp.begin(); 218 | temp.erase(it); 219 | } 220 | temp.clear(); 221 | 222 | Plane pl_; 223 | PointCloud randcloud_; 224 | for (int i = 0; i < randnum; i++) 225 | { 226 | randcloud_.push_back(cloud[nIndex[i]]); 227 | } 228 | pl_.computeFromPoints(randcloud_); 229 | 230 | 231 | inliers_n = 0; 232 | for (size_t i = 0; i < cloud.size(); ++i) 233 | { 234 | double d2plane = pl_.point2plane(cloud[i]); 235 | if (d2plane <= threshold) 236 | { 237 | inliers_n++; 238 | } 239 | } 240 | 241 | //更新假设点集先验概率 242 | for (size_t i = 0; i < cloud.size(); ++i) 243 | { 244 | double d2plane = pl_.point2plane(cloud[i]); 245 | if (d2plane <= threshold) 246 | { 247 | pro[i] = pro[i] * (double)inliers_n / cloud.size(); 248 | } 249 | } 250 | 251 | 252 | //寻找局内点 253 | if (inliers_n > inliers_l) 254 | { 255 | inliers_l = inliers_n; 256 | initial_iter = int(ceil(log10(0.01) / log10(1.0 - pow((double)inliers_n / cloud.size(), 3))));//更新K值 257 | iter = 0; 258 | model.setPara(pl_.getA(), pl_.getB(), pl_.getC(), pl_.getD()); 259 | continue;//进行下一次循环 260 | } 261 | iter++; 262 | } 263 | 264 | delete[]nIndex; 265 | inliers_threshold = threshold; 266 | return true; 267 | } 268 | 269 | 270 | bool td::PlaneFitting::computeByLMedS() 271 | { 272 | srand((unsigned)time(NULL)); 273 | if (cloud.size() <= 3) 274 | return false; 275 | 276 | int randnum = model.needNum; 277 | int* nIndex = new int[randnum]; //随机点索引 278 | //参数统计步骤已经结束 279 | int initial_iter = 100; 280 | int iter(0); //RANSAC循环变量 281 | double min_median(1000), mid_deviation(0), bstd(0);//最小中值，本次循环局内点数目 282 | 283 | 284 | //？循环次数和概率的更新还是需要阈值 285 | while (iter < initial_iter) 286 | { 287 | //找出概率值最高的三个点 288 | for (int i = 0; i < randnum; i++) 289 | { 290 | nIndex[i] = rand() % cloud.size(); 291 | } 292 | 293 | //判断抽取随机点是否重复 294 | bool allsame(true); 295 | for (int i = 1; i < randnum; i++) 296 | { 297 | allsame = allsame&&nIndex[i] == nIndex[i - 1]; 298 | } 299 | if (allsame) 300 | continue; 301 | 302 | Plane pl_; 303 | PointCloud randcloud_; 304 | for (int i = 0; i < randnum; i++) 305 | { 306 | randcloud_.push_back(cloud[nIndex[i]]); 307 | } 308 | 309 | pl_.computeFromPoints(randcloud_); 310 | 311 | std::vector model_deviation_p(0);//统计偏差中值 312 | 313 | mid_deviation = 0;//中值 314 | 315 | for (size_t i = 0; i < cloud.size(); ++i) 316 | { 317 | double d2plane = pl_.point2plane(cloud[i]); 318 | model_deviation_p.push_back(d2plane); 319 | } 320 | 321 | sort(model_deviation_p.begin(), model_deviation_p.end()); 322 | if (model_deviation_p.size() % 2 == 0) 323 | mid_deviation = (model_deviation_p[model_deviation_p.size() / 2 - 1] + 324 | model_deviation_p[model_deviation_p.size() / 2]) / 2; 325 | else 326 | mid_deviation = model_deviation_p[model_deviation_p.size() / 2]; 327 | 328 | //更新假设点集先验概率 329 | double std_ = 0; 330 | std_ = computeStd(pl_, cloud); 331 | 332 | //寻找最小中值，并计算模型参数 333 | if (mid_deviation < min_median) 334 | { 335 | min_median = mid_deviation; 336 | int inliers = 0; 337 | 338 | for (size_t i = 0; i < cloud.size(); ++i) 339 | { 340 | double d2plane = pl_.point2plane(cloud[i]); 341 | if (d2plane <= 2 * std_) 342 | { 343 | inliers++; 344 | } 345 | } 346 | bstd = std_; 347 | model.setPara(pl_.getA(), pl_.getB(), pl_.getC(), pl_.getD()); 348 | initial_iter = int(ceil(log10(0.01) / log10(1.0 - pow((double)inliers / cloud.size(), 3)))); 349 | iter = 0; 350 | continue; 351 | } 352 | iter++; 353 | } 354 | 355 | 356 | delete[]nIndex; 357 | inliers_threshold = 2 * bstd; 358 | return true; 359 | } 360 | 361 | 362 | //BAYLMEDS进行拟合 363 | bool td::PlaneFitting::computeByBayLMedS() 364 | { 365 | srand((unsigned)time(NULL)); 366 | if (cloud.size() <= 3) 367 | return false; 368 | 369 | //初始迭代次数 370 | int initial_iter = int(ceil(log10(0.01) / log10(1.0 - (double)model.needNum / cloud.size()))); 371 | int stas_num = int(0.1*(double)initial_iter); 372 | std::vector pro(cloud.size(), 0);//概率值，索引与点一一对应 373 | int randnum = model.needNum; 374 | 375 | 376 | double para_percent = 0;//最优参数占总参数的比例 377 | 378 | //用于参数统计 379 | 380 | Plane stat_best_plane; 381 | double stat_min_penalty(std::numeric_limits::max()); 382 | 383 | //个人感觉进行参数统计不靠谱，通过寻找最优参数来确定先验概率 384 | for (int iter = 0; iter < 6;) 385 | { 386 | std::set nIndex;//随机点索引 387 | for (int i = 0; i < randnum; i++) 388 | { 389 | int curIndex = rand() % cloud.size(); 390 | nIndex.insert(curIndex); 391 | } 392 | //判断随机抽取的点是否重复 393 | if (nIndex.size() < randnum) 394 | continue; 395 | 396 | Plane pl; 397 | PointCloud randcloud; 398 | std::set::const_iterator it_nIndex; 399 | for (it_nIndex = nIndex.begin(); it_nIndex != nIndex.end(); ++it_nIndex) 400 | { 401 | randcloud.push_back(cloud[*it_nIndex]); 402 | } 403 | pl.computeFromPoints(randcloud); 404 | 405 | std::vector model_deviation(0);//统计偏差中值 406 | 407 | double cur_penalty = 0; 408 | for (size_t i = 0; i < cloud.size(); i++) 409 | { 410 | double d2plane = pl.point2plane(cloud[i]); 411 | model_deviation.push_back(d2plane); 412 | } 413 | 414 | sort(model_deviation.begin(), model_deviation.end()); 415 | 416 | //获取中值 417 | if (model_deviation.size() % 2 == 0) 418 | cur_penalty = (model_deviation[model_deviation.size() / 2 - 1] + 419 | model_deviation[model_deviation.size() / 2]) / 2; 420 | else 421 | cur_penalty = model_deviation[model_deviation.size() / 2]; 422 | 423 | //更新统计中的最优中值 424 | if (cur_penalty < stat_min_penalty) 425 | { 426 | stat_min_penalty = cur_penalty; 427 | stat_best_plane = pl; 428 | } 429 | 430 | iter++; 431 | } 432 | 433 | 434 | //贝叶斯过程 435 | /*std::vector::iterator it_f = plnum.begin(); 436 | std::vector::iterator it_s = max_element(plnum.begin(), plnum.end()); 437 | size_t para_best_index = it_s - it_f;*/ 438 | 439 | //通过参数统计为点赋先验概率 440 | //计算标准差 441 | //double std = computeStd(planes[para_best_index], cloud); 442 | double th = computeMStd(stat_best_plane, cloud, stat_min_penalty); 443 | for (size_t i = 0; i < cloud.size(); i++) 444 | { 445 | double d2plane = stat_best_plane.point2plane(cloud[i]); 446 | 447 | if (d2plane <= th) 448 | { 449 | pro[i] = 1 - d2plane / th; 450 | } 451 | } 452 | 453 | //参数统计步骤已经结束 454 | 455 | //double best_median(1000), mid_deviation(0), bstd(0);//最小中值，本次循环局内点数目 456 | double best_penalty(std::numeric_limits::max()); 457 | double best_inliers(0); 458 | double best_th(th); 459 | 460 | //？循环次数和概率的更新还是需要阈值 461 | std::vector g_index(randnum, 0); 462 | int iter(0); //初始循环变量 463 | while (iter < initial_iter) 464 | { 465 | //找出概率值最高的三个点 466 | std::vector cur_index(randnum, 0); 467 | std::vector temp = pro; 468 | 469 | for (int i = 0; i < randnum; i++) 470 | { 471 | std::vector::iterator it; 472 | it = max_element(temp.begin(), temp.end()); 473 | int iIndex = it - temp.begin(); 474 | cur_index[i] = iIndex; 475 | temp.erase(it); 476 | } 477 | 478 | bool goon = true; 479 | 480 | //排序 481 | std::sort(cur_index.begin(), cur_index.end()); 482 | std::sort(g_index.begin(), g_index.end()); 483 | 484 | for (int i = 0; i < randnum; i++) 485 | { 486 | goon = goon&&cur_index[i] == g_index[i]; 487 | } 488 | 489 | if (goon&&iter>0) 490 | break; 491 | 492 | for (int i = 0; i < randnum; i++) 493 | { 494 | g_index[i] = cur_index[i]; 495 | } 496 | 497 | Plane pl_; 498 | PointCloud randcloud_; 499 | for (int i = 0; i < randnum; i++) 500 | { 501 | randcloud_.push_back(cloud[cur_index[i]]); 502 | } 503 | 504 | pl_.computeFromPoints(randcloud_); 505 | 506 | std::vector model_deviation_p(0);//统计偏差中值 507 | 508 | //mid_deviation = 0;//中值 509 | double cur_penalty(0); 510 | 511 | for (size_t i = 0; i < cloud.size(); ++i) 512 | { 513 | double d2plane = pl_.point2plane(cloud[i]); 514 | model_deviation_p.push_back(d2plane); 515 | } 516 | 517 | sort(model_deviation_p.begin(), model_deviation_p.end()); 518 | if (model_deviation_p.size() % 2 == 0) 519 | cur_penalty = (model_deviation_p[model_deviation_p.size() / 2 - 1] + 520 | model_deviation_p[model_deviation_p.size() / 2]) / 2; 521 | else 522 | cur_penalty = model_deviation_p[model_deviation_p.size() / 2]; 523 | 524 | //更新假设点集先验概率 525 | double k = 0; //k为当前中值，局内点的个数 526 | double cur_th = 0; 527 | cur_th = computeMStd(pl_, cloud, cur_penalty); 528 | 529 | for (size_t i = 0; i < cloud.size(); ++i) 530 | { 531 | double d2plane = pl_.point2plane(cloud[i]); 532 | if (d2plane <= best_th) 533 | { 534 | k = k + 1.0; 535 | } 536 | } 537 | 538 | for (size_t i = 0; i < cloud.size(); ++i) 539 | { 540 | double d2plane = pl_.point2plane(cloud[i]); 541 | if (d2plane <= best_th) 542 | { 543 | pro[i] = pro[i] * k / cloud.size(); 544 | } 545 | } 546 | 547 | //寻找最小中值，并计算模型参数 548 | if (cur_penalty < best_penalty) 549 | { 550 | best_penalty = cur_penalty; 551 | best_inliers = k; 552 | best_th = cur_th; 553 | /*for (size_t i = 0; i < cloud.size(); ++i) 554 | { 555 | double d2plane = pl_.point2plane(cloud[i]); 556 | if (d2plane <= kk * std_) 557 | { 558 | inliers++; 559 | } 560 | } 561 | bstd = std_; 562 | model.setPara(pl_.getA(), pl_.getB(), pl_.getC(), pl_.getD());*/ 563 | model = pl_; 564 | //initial_iter = int(ceil(log10(0.01) / log10(1.0 - pow((double)best_inliers / cloud.size(), 3)))); 565 | //iter = 0; 566 | } 567 | iter++; 568 | } 569 | 570 | inliers_threshold = best_th; 571 | return true; 572 | } 573 | 574 | // 返回平面参数 575 | td::Plane td::PlaneFitting::getModel() 576 | { 577 | return model; 578 | } 579 | 580 | 581 | // 返回局内点 582 | td::PointCloud td::PlaneFitting::getInliers() 583 | { 584 | PointCloud inliers; 585 | for (size_t i = 0; i < cloud.size(); ++i) 586 | { 587 | double d2plane = model.point2plane(cloud[i]); 588 | if (d2plane <= inliers_threshold) 589 | { 590 | inliers.push_back(cloud[i]); 591 | } 592 | } 593 | return inliers; 594 | } 595 | 596 | 597 | // 返回模型标准阈值 598 | double td::PlaneFitting::getInlierThershold() 599 | { 600 | return inliers_threshold; 601 | } 602 | 603 | 604 | // 返回局外点 605 | td::PointCloud td::PlaneFitting::getOutliers() 606 | { 607 | PointCloud outliers; 608 | for (size_t i = 0; i < cloud.size(); ++i) 609 | { 610 | double d2plane = model.point2plane(cloud[i]); 611 | if (d2plane > inliers_threshold) 612 | { 613 | outliers.push_back(cloud[i]); 614 | } 615 | } 616 | return outliers; 617 | } 618 | 619 | 620 | // 计算点到面的标准差 621 | double td::PlaneFitting::computeStd(Plane& pl, PointCloud& cloud) 622 | { 623 | double avg_d(0), avg_d2(0); 624 | for (size_t i = 0; i < cloud.size(); i++) 625 | { 626 | double d2plane = pl.point2plane(cloud[i]); 627 | avg_d += d2plane; 628 | avg_d2 += pow(d2plane, 2.0); 629 | } 630 | double ptnum = (double)cloud.size(); 631 | /*avg_d /= ptnum; 632 | avg_d2 /= ptnum;*/ 633 | double std = sqrt(avg_d2/ptnum - pow(avg_d/ptnum, 2.0)); 634 | return std; 635 | } 636 | 637 | // 最小中值对应的标准差,适用于最小中值中阈值的计算 638 | // derta = 1.4856[1 + 5/(n-p)]/sqrt(Mj) 639 | double td::PlaneFitting::computeMStd(Plane& pl, PointCloud& cloud, double penalty) 640 | { 641 | double num = (double)cloud.size(); 642 | double sigma = 1.4826 * (1 + 5.0 / (num - 3.0))*penalty; 643 | return 2.5*sigma; 644 | } 645 | -------------------------------------------------------------------------------- /PlaneFitting.h: -------------------------------------------------------------------------------- 1 | /* 2 | *主要用于点云的拟合 3 | *LMS：最小二乘法 4 | *RANSAC：随机抽样一致性 5 | *BaySAC：基于贝叶斯先验概率抽样一致性 6 | *BayLMedS:基于贝叶斯先验概率最小平方中值 7 | *罗中飞，2015.12 8 | */ 9 | 10 | 11 | 12 | #ifndef fitting_h 13 | #define fitting_h 14 | 15 | 16 | #include "Point.h" 17 | #include "Plane.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace td 24 | { 25 | 26 | class PlaneFitting 27 | { 28 | public: 29 | PlaneFitting(); 30 | PlaneFitting(const td::PointCloud& pc); 31 | ~PlaneFitting(); 32 | 33 | //输入点云 34 | void setInputCloud(const td::PointCloud& pc); 35 | 36 | 37 | //RANSAC进行拟合 38 | bool computeByRANSAC(double ); 39 | 40 | //BAYSAC进行拟合 41 | bool computeByBAYSAC(double ); 42 | 43 | //BAYLMEDS进行拟合 44 | bool computeByBayLMedS(); 45 | 46 | // LMEDS 47 | bool computeByLMedS(); 48 | 49 | private: 50 | PointCloud cloud; 51 | Plane model; 52 | public: 53 | // 返回平面参数 54 | Plane getModel(); 55 | private: 56 | // 局内点阈值 57 | double inliers_threshold; 58 | public: 59 | // 返回局内点 60 | PointCloud getInliers(); 61 | // 返回模型标准阈值 62 | double getInlierThershold(); 63 | // 返回局外点 64 | td::PointCloud getOutliers(); 65 | 66 | private: 67 | // 计算点到面的标准差 68 | double computeStd(Plane& pl, PointCloud& cloud); 69 | 70 | // 最小中值对应的标准差 71 | // derta = 1.4826[1 + 5/(n-p)]/sqrt(Mj) 72 | double computeMStd(Plane& pl, PointCloud& cloud, double penalty); 73 | }; 74 | } 75 | 76 | #endif; 77 | -------------------------------------------------------------------------------- /Point.cpp: -------------------------------------------------------------------------------- 1 | #include "Point.h" 2 | #include 3 | 4 | td::Point::Point(): 5 | x(0), 6 | y(0), 7 | z(0) 8 | { 9 | 10 | } 11 | 12 | td::Point::Point(double x,double y,double z) 13 | { 14 | this->x = x; 15 | this->y = y; 16 | this->z = z; 17 | } 18 | 19 | td::Point::~Point() 20 | { 21 | } 22 | 23 | //设置点的属性 24 | void td::Point::setPoint(double x, double y, double z) 25 | { 26 | this->x = x; 27 | this->y = y; 28 | this->z = z; 29 | } 30 | 31 | 32 | bool td::Point::operator==(const Point& pnt) 33 | { 34 | if (x == pnt.x) 35 | if (y == pnt.y) 36 | if (z == pnt.z) 37 | return true; 38 | return false; 39 | } 40 | -------------------------------------------------------------------------------- /Point.h: -------------------------------------------------------------------------------- 1 | /* 2 | *基本点类型 3 | *罗中飞，2015.12 4 | */ 5 | 6 | #ifndef point_h 7 | #define point_h 8 | 9 | #include 10 | #include 11 | 12 | namespace td 13 | { 14 | class Point 15 | { 16 | public: 17 | Point(); 18 | //构造函数:Point(x,y,z) 19 | Point(double, double, double); 20 | ~Point(); 21 | 22 | //可以用来sort和作为map的key 23 | friend bool operator<(const Point &_pnta, const Point &_pntb) 24 | { 25 | if (_pnta.x<_pntb.x){ 26 | return true; 27 | } 28 | else if (_pnta.x>_pntb.x){ 29 | return false; 30 | } 31 | else if (_pnta.y<_pntb.y){ 32 | return true; 33 | } 34 | else if (_pnta.y>_pntb.y){ 35 | return false; 36 | } 37 | else if (_pnta.z<_pntb.z){ 38 | return true; 39 | } 40 | else if (_pnta.z>_pntb.z){ 41 | return false; 42 | } 43 | else{ 44 | return false; 45 | } 46 | } 47 | 48 | //设置点的属性 49 | void setPoint(double, double, double); 50 | 51 | 52 | public: 53 | double x; 54 | double y; 55 | double z; 56 | bool operator==(const Point& pnt); 57 | }; 58 | 59 | typedef std::vector PointCloud; 60 | typedef std::shared_ptr Ptr; 61 | } 62 | #endif; 63 | -------------------------------------------------------------------------------- /PointIO.cpp: -------------------------------------------------------------------------------- 1 | #include "PointIO.h" 2 | 3 | 4 | td::PointIO::PointIO() 5 | : cloud(0) 6 | { 7 | } 8 | 9 | 10 | td::PointIO::PointIO(std::string path) 11 | { 12 | std::ifstream infile; 13 | infile.open(path.c_str(), std::ios::in); 14 | if (!infile) 15 | { 16 | std::cerr << "文件有误" << std::endl; 17 | return; 18 | } 19 | std::string line, str; 20 | //暂时读入x,y,z形式的文件 21 | while (!infile.eof()) 22 | { 23 | /*getline(infile,line); 24 | istringstream stream(line); 25 | 26 | stream>>tempx>>str>>tempy>>str>>tempz;*/ 27 | 28 | Point point; 29 | infile >> point.x; 30 | infile >> point.y; 31 | infile >> point.z; 32 | cloud.push_back(point); 33 | } 34 | infile.close(); 35 | } 36 | 37 | td::PointIO::~PointIO() 38 | { 39 | } 40 | 41 | 42 | td::PointCloud td::PointIO::getPointCloud() 43 | { 44 | return cloud; 45 | } 46 | 47 | 48 | // 读入文件 49 | void td::PointIO::open(std::string path) 50 | { 51 | std::ifstream infile; 52 | infile.open(path.c_str(), std::ios::in); 53 | if (!infile) 54 | { 55 | std::cerr << "文件有误" << std::endl; 56 | return; 57 | } 58 | std::string line, str; 59 | //暂时读入x,y,z形式的文件 60 | while (!infile.eof()) 61 | { 62 | 63 | Point point; 64 | infile >> point.x; 65 | infile >> point.y; 66 | infile >> point.z; 67 | cloud.push_back(point); 68 | } 69 | infile.close(); 70 | } 71 | 72 | 73 | void td::PointIO::save(std::string path) 74 | { 75 | std::ofstream ofile; 76 | ofile.open(path.c_str(), std::ios::out); 77 | if (!ofile) 78 | { 79 | std::cerr << "文件有误" << std::endl; 80 | return; 81 | } 82 | //暂时读入x,y,z形式的文件 83 | for (size_t i = 0; i < cloud.size();++i) 84 | { 85 | 86 | ofile << cloud[i].x; 87 | ofile << cloud[i].y; 88 | ofile << cloud[i].z; 89 | } 90 | ofile.close(); 91 | } 92 | -------------------------------------------------------------------------------- /PointIO.h: -------------------------------------------------------------------------------- 1 | /* 2 | *点云的读取 3 | *罗中飞，2015.12 4 | */ 5 | 6 | #ifndef pointio_h 7 | #define pointio_h 8 | 9 | #include "Point.h" 10 | #include 11 | #include 12 | #include 13 | 14 | namespace td 15 | { 16 | class PointIO 17 | { 18 | public: 19 | PointIO(); 20 | PointIO(std::string ); 21 | ~PointIO(); 22 | private: 23 | // 点云数据 24 | PointCloud cloud; 25 | public: 26 | PointCloud getPointCloud(); 27 | // 读入文件 28 | void open(std::string path); 29 | void save(std::string path); 30 | }; 31 | } 32 | #endif 33 | 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Segmentation 2 | PointCloud Segmentaion 3 | Mainly plane segmentation by RANSAC, BAYSAC, BAYLMedS, LMedS 4 | -------------------------------------------------------------------------------- /Segmentation.cpp: -------------------------------------------------------------------------------- 1 | #include "Segmentation.h" 2 | 3 | 4 | 5 | td::Segmentation::Segmentation() 6 | : radius(0) 7 | , threshold(0) 8 | , k(0) 9 | , averageError(0) 10 | { 11 | } 12 | 13 | td::Segmentation::Segmentation(const PointCloud& in) 14 | { 15 | cloud = in; 16 | } 17 | 18 | td::Segmentation::~Segmentation() 19 | { 20 | } 21 | 22 | void td::Segmentation::setInputCloud(const PointCloud& in) 23 | { 24 | cloud = in; 25 | } 26 | 27 | //区域生长分割,RANSAC拟合 28 | void td::Segmentation::regionGrow(METHOD method, int k) 29 | { //建立树 30 | 31 | 32 | //Step1:准备k邻域数据 33 | KdTree tree(cloud); 34 | //建立空间坐标索引 35 | /*std::map index_map;*/ 36 | std::map label; 37 | 38 | //标签初始化 39 | for (size_t index = 0; index < cloud.size(); ++index) 40 | { 41 | label[cloud[index]] = false; 42 | /*index_map[cloud[index]] = index;*/ 43 | } 44 | 45 | //随机种子点 46 | for (size_t index = 0; index < cloud.size();index++) 47 | { 48 | if (label[cloud[index]]) 49 | continue; 50 | PointCloud neighbors; //邻域点集 51 | tree.setNumberOfNeighbours(k); 52 | //tree.setNeighboursRadius(radius);//邻域半径 53 | tree.kNearestNeighbor(cloud[index].x, cloud[index].y, cloud[index].z); 54 | neighbors = tree.getNearestNeighbor(); 55 | 56 | //开始进行拟合,找到种子面 57 | PlaneFitting fit; 58 | fit.setInputCloud(neighbors); 59 | bool state(false); 60 | state = selectMethod(fit, method); 61 | if (!state) 62 | continue; 63 | //获取平面参数，及局内点 64 | Plane pl; 65 | pl = fit.getModel(); 66 | PointCloud inliers; 67 | inliers = fit.getInliers(); 68 | 69 | //对邻域点进行标记 70 | for (size_t in_i = 0; in_i < inliers.size(); ++in_i) 71 | { 72 | label[inliers[in_i]] = true; 73 | } 74 | 75 | //对局内点进行区域生长 76 | int n = 0; 77 | //最好不要使用迭代器进行遍历 78 | for (size_t i = 0; i < inliers.size(); ++i) 79 | { 80 | tree.kNearestNeighbor(inliers[i].x, inliers[i].y, inliers[i].z); 81 | PointCloud candidate = tree.getNearestNeighbor(); 82 | for (PointCloud::iterator can_it = candidate.begin(); can_it != candidate.end(); ++can_it) 83 | { 84 | double dis = pl.point2plane(*can_it); 85 | if (dis > fit.getInlierThershold() || label[*can_it] == true) 86 | { 87 | continue; 88 | } 89 | ++n; 90 | inliers.push_back(*can_it); //加入局内点 91 | label[*can_it] = true; 92 | if (n % k == 0) 93 | { 94 | fit.setInputCloud(inliers); 95 | state = selectMethod(fit, method); 96 | pl = fit.getModel(); //更新平面参数 97 | } 98 | } 99 | } 100 | indices.push_back(inliers); 101 | plane_models.push_back(pl); 102 | double currentError = 0; 103 | for (size_t i = 0; i < inliers.size(); i++) 104 | { 105 | currentError += pow(pl.point2plane(inliers[i]), 2); 106 | } 107 | currentError /= inliers.size(); 108 | currentError = sqrt(currentError); 109 | averageError += currentError; 110 | } 111 | averageError /= indices.size(); 112 | } 113 | 114 | int td::Segmentation::getModelNum() 115 | { 116 | return indices.size(); 117 | } 118 | 119 | 120 | // 设置邻域搜索半径 121 | void td::Segmentation::setRadius(double r) 122 | { 123 | this->radius = r; 124 | } 125 | 126 | 127 | // 设置拟合阈值，主要针对RANSAC和BAYSAC拟合 128 | void td::Segmentation::setThreshold(double threshold) 129 | { 130 | this->threshold = threshold; 131 | } 132 | 133 | 134 | bool td::Segmentation::selectMethod(PlaneFitting& fit, METHOD method) 135 | { 136 | bool state; 137 | switch (method) 138 | { 139 | case RANSAC: 140 | state = fit.computeByRANSAC(threshold); 141 | break; 142 | case BAYSAC: 143 | state = fit.computeByBAYSAC(threshold); 144 | break; 145 | case LMEDS: 146 | state = fit.computeByLMedS(); 147 | break; 148 | case BAYLMEDS: 149 | state = fit.computeByBayLMedS(); 150 | break; 151 | default: 152 | break; 153 | } 154 | return state; 155 | } 156 | 157 | 158 | // 点云分割拟合后输出 159 | void td::Segmentation::outPut(std::string path) 160 | { 161 | std::ofstream outfile; 162 | outfile.open(path, std::ios::out); 163 | 164 | srand((unsigned)time(NULL)); 165 | for (size_t i = 0; i < indices.size(); i++) 166 | { 167 | //附上随机颜色 168 | unsigned int r = (int)((double)rand() / ((RAND_MAX + 1.0) / (255 - 100 + 1.0)) + 100); 169 | unsigned int g = (int)((double)rand() / ((RAND_MAX + 1.0) / (255 - 100 + 1.0)) + 100); 170 | unsigned int b = (int)((double)rand() / ((RAND_MAX + 1.0) / (255 - 100 + 1.0)) + 100); 171 | for (size_t j = 0; j < indices.at(i).size(); j++) 172 | { 173 | outfile << std::setw(15); 174 | outfile << std::right; 175 | outfile << std::fixed; 176 | outfile << indices.at(i).at(j).x; 177 | outfile << std::setw(15); 178 | outfile << std::right; 179 | outfile << indices.at(i).at(j).y; 180 | outfile << std::setw(15); 181 | outfile << std::right; 182 | outfile << indices.at(i).at(j).z; 183 | outfile << std::setw(6); 184 | outfile << std::right; 185 | outfile << r; 186 | outfile << std::setw(6); 187 | outfile << std::right; 188 | outfile << g; 189 | outfile << std::setw(6); 190 | outfile << std::right; 191 | outfile << b; 192 | outfile << std::endl; 193 | } 194 | } 195 | outfile.close(); 196 | 197 | } 198 | 199 | 200 | // 设置k邻域值 201 | void td::Segmentation::setKNearest(int k) 202 | { 203 | this->k = k; 204 | } 205 | 206 | double td::Segmentation::getAverageError() 207 | { 208 | return averageError; 209 | } 210 | 211 | const std::vector &td::Segmentation::getIndices() 212 | { 213 | return indices; 214 | } 215 | 216 | const std::vector& td::Segmentation::getPlaneModels() 217 | { 218 | return plane_models; 219 | } 220 | 221 | // //Multi-RANSAC进行分割 222 | void td::Segmentation::multiRansac() 223 | { 224 | indices.clear(); 225 | double pro = 1.0; 226 | PointCloud mcloud = cloud; 227 | while (pro>0.2) 228 | { 229 | PlaneFitting fit; 230 | fit.setInputCloud(mcloud); 231 | bool state(false); 232 | state = selectMethod(fit, RANSAC); 233 | if (!state) 234 | break; 235 | //获取平面参数，及局内点 236 | Plane pl; 237 | pl = fit.getModel(); 238 | PointCloud inliers; 239 | inliers = fit.getInliers(); 240 | indices.push_back(inliers); 241 | plane_models.push_back(pl); 242 | mcloud = fit.getOutliers(); 243 | pro = (double)mcloud.size() / cloud.size(); 244 | double currentError = 0; 245 | for (size_t i = 0; i < inliers.size(); i++) 246 | { 247 | currentError += pow(pl.point2plane(inliers[i]), 2); 248 | } 249 | currentError /= inliers.size(); 250 | currentError = sqrt(currentError); 251 | averageError += currentError; 252 | } 253 | averageError /= indices.size(); 254 | } 255 | 256 | 257 | // 单模型拟合 258 | void td::Segmentation::singleFitting(METHOD method, Plane& pl) 259 | { 260 | PlaneFitting fit; 261 | fit.setInputCloud(cloud); 262 | bool state(false); 263 | state = selectMethod(fit, method); 264 | 265 | //获取平面参数，及局内点 266 | // Plane pl; 267 | pl = fit.getModel(); 268 | PointCloud inliers; 269 | inliers = fit.getInliers(); 270 | indices.push_back(inliers); 271 | double currentError = 0; 272 | for (size_t i = 0; i < inliers.size(); i++) 273 | { 274 | currentError += pow(pl.point2plane(inliers[i]), 2); 275 | } 276 | currentError /= inliers.size(); 277 | currentError = sqrt(currentError); 278 | averageError += currentError; 279 | averageError /= indices.size(); 280 | } 281 | -------------------------------------------------------------------------------- /Segmentation.h: -------------------------------------------------------------------------------- 1 | /* 2 | *对点云进行分割 3 | */ 4 | 5 | #ifndef segmentation_h 6 | #define segmentation_h 7 | 8 | #include "Point.h" 9 | #include "PlaneFitting.h" 10 | #include "Plane.h" 11 | 12 | #include "kdtree.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace td 22 | { 23 | class Segmentation 24 | { 25 | public: 26 | Segmentation(); 27 | Segmentation(const PointCloud&); 28 | ~Segmentation(); 29 | 30 | enum METHOD{RANSAC,BAYSAC,LMEDS,BAYLMEDS}; 31 | 32 | //设置点云 33 | void setInputCloud(const PointCloud&); 34 | 35 | //区域生长分割 36 | void regionGrow(METHOD , int); 37 | 38 | // multi-RANSAC分割 39 | void multiRansac(); 40 | 41 | // 单模型拟合 42 | void singleFitting(METHOD , Plane&); 43 | 44 | // 设置邻域搜索半径 45 | void setRadius(double r); 46 | 47 | // 设置拟合阈值，主要针对RANSAC和BAYSAC拟合 48 | void setThreshold(double threshold); 49 | 50 | // 点云分割拟合后输出 51 | void outPut(std::string); 52 | 53 | // 设置k邻域值 54 | void setKNearest(int); 55 | 56 | // 返回模型个数 57 | int getModelNum(); 58 | 59 | // 返回平均误差 60 | double getAverageError(); 61 | 62 | // 返回inliers indices 63 | const std::vector& getIndices(); 64 | 65 | // 返回面片模型参数 66 | const std::vector& getPlaneModels(); 67 | 68 | private: 69 | // k邻域值 70 | int k; 71 | 72 | // 平均拟合误差 73 | double averageError; 74 | 75 | PointCloud cloud;//点云 76 | std::vector indices;//分割后的点云面片 77 | 78 | std::vector plane_models; //分割模型参数 79 | 80 | double radius;//邻域搜索半径 81 | 82 | // 拟合阈值,主要针对RANSAC和BaySAC 83 | double threshold; 84 | 85 | bool selectMethod(PlaneFitting& fit, METHOD method); 86 | 87 | }; 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /kdtree.cpp: -------------------------------------------------------------------------------- 1 | #include "kdtree.h" 2 | int td::KdTree::kNearestNeighbor(double &_x,double &_y,double &_z) 3 | { 4 | pnts_.clear(); 5 | nNearestNodes nNN(numberOfNeighbours_); 6 | nNN.setDistandIndx(radius_); 7 | nNN.setSearchPnt(_x,_y,_z); 8 | t_.locateNodes(&nNN,1); 9 | //cout<<"找到的点数："<pos[0]; 15 | y=nNN.index[i]->pos[1]; 16 | z=nNN.index[i]->pos[2]; 17 | p.setPoint(x, y, z); 18 | pnts_.push_back(p); 19 | } 20 | return pnts_.size(); //返回最临近点个数 21 | } -------------------------------------------------------------------------------- /kdtree.h: -------------------------------------------------------------------------------- 1 | #ifndef KDTREE_H 2 | #define KDTREE_H 3 | 4 | 5 | #include 6 | #include 7 | #include "3dKDtree.h" 8 | #include "Point.h" 9 | 10 | namespace td 11 | { 12 | class KdTree 13 | { 14 | kdTree t_; 15 | int numberOfNeighbours_; //按最近邻域点个数搜索 16 | double radius_; //按半径搜索 17 | PointCloud pnts_; //存放最近邻域点 18 | public: 19 | KdTree(PointCloud &_p) :t_(_p.size()) //建立Kdtree,传如的参数是list数据,继承自Point类，其中有xyz值 20 | { 21 | std::vector::iterator it; 22 | it = _p.begin(); 23 | 24 | for (int i = 0; it != _p.end(); ++it, ++i) 25 | { 26 | t_.store(it->x, it->y, it->z, i); 27 | } 28 | t_.treeBalance(); //调整树 29 | } 30 | 31 | void setNumberOfNeighbours(int _num) //按最近邻域点搜索 32 | { 33 | numberOfNeighbours_ = _num; 34 | radius_ = 5.0; //默认1.0米,具有500+个点，搜索半径太大没意义 35 | } 36 | 37 | void setNeighboursRadius(double _radius) //按半径搜索 38 | { 39 | radius_ = _radius; 40 | numberOfNeighbours_ = 1000; //1平米点数约有700+点 41 | } 42 | int kNearestNeighbor(double &_x, double &_y, double &_z); //搜索最近n个邻域点,其本身也被包括在内&返回最临近点个数 43 | 44 | std::vector& getNearestNeighbor() //取最临近值 45 | { 46 | return pnts_; 47 | } 48 | }; 49 | } 50 | #endif -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | *分割拟合算法测试主程序 3 | *罗中飞，2015.12 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "PointIO.h" 11 | #include "Segmentation.h" 12 | 13 | #include 14 | 15 | void pointCloud2pclPointCloud(const td::PointCloud& cloud, pcl::PointCloud::Ptr& pclCloud) 16 | { 17 | if(!pclCloud->empty()) 18 | { 19 | pclCloud.reset(); 20 | } 21 | for (int i = 0; i < cloud.size(); i++) 22 | { 23 | pcl::PointXYZ pt; 24 | pt.x = cloud[i].x; 25 | pt.y = cloud[i].y; 26 | pt.z = cloud[i].z; 27 | pclCloud->push_back(pt); 28 | } 29 | } 30 | 31 | void pclPointCloud2PointCloud(const pcl::PointCloud::Ptr& pclCloud, td::PointCloud& cloud) 32 | { 33 | if(!cloud.empty()) 34 | cloud.clear(); 35 | for (int i = 0; i < pclCloud->size(); i++) 36 | { 37 | td::Point pt; 38 | pt.x = pclCloud->at(i).x; 39 | pt.y = pclCloud->at(i).y; 40 | pt.z = pclCloud->at(i).z; 41 | cloud.push_back(pt); 42 | } 43 | } 44 | 45 | int main() 46 | { 47 | std::cout << "------------------------分割拟合程序开始运行---------------------"<< std::endl; 48 | std::cout << "输入点云文件路径（不能带有空格）"<> path; 52 | 53 | td::PointIO ptIO; 54 | ptIO.open(path); 55 | 56 | td::PointCloud cloud; 57 | pcl::PointCloud::Ptr pclCloud(new pcl::PointCloud); 58 | pcl::PointCloud::Ptr filteredCloud(new pcl::PointCloud); 59 | pointCloud2pclPointCloud(ptIO.getPointCloud(), pclCloud); 60 | std::cout << "输入抽稀网格大小:" << std::endl; 61 | double leafSize(0); 62 | std::cin >> leafSize; 63 | pcl::VoxelGrid filter; 64 | filter.setInputCloud(pclCloud); 65 | filter.setLeafSize(leafSize, leafSize, leafSize); 66 | filter.filter(*filteredCloud); 67 | pclPointCloud2PointCloud(filteredCloud, cloud); 68 | 69 | 70 | td::Segmentation seg; 71 | seg.setInputCloud(cloud); 72 | 73 | std::cout << "k邻域值(根据实际点云密度进行选择)" << std::endl; 74 | int k; 75 | std::cin >> k; 76 | seg.setKNearest(k); 77 | 78 | /*std::cout << "邻域半径" << std::endl; 79 | double radius; 80 | std::cin >> radius; 81 | seg.setRadius(radius);*/ 82 | 83 | std::cout << "平面拟合阈值" << std::endl; 84 | double threshold; 85 | std::cin >> threshold; 86 | seg.setThreshold(threshold); 87 | 88 | // std::cout << "------------------------输入拟合方法---------------------" << std::endl; 89 | // std::cout << "------------------------0：RANSAC---------------------" << std::endl; 90 | // std::cout << "------------------------1：BAYSAC---------------------" << std::endl; 91 | // std::cout << "------------------------2：LMEDS---------------------" << std::endl; 92 | // std::cout << "------------------------3：BAYLMEDS---------------------" << std::endl; 93 | // std::cout << "------------------------开始生长拟合---------------------" << std::endl; 94 | // int me; 95 | // std::cin >> me; 96 | 97 | clock_t start, end; 98 | start = clock(); 99 | std::cout << "拟合中..." <> minN; 114 | // std::cout << "计算挡板中..." << std::endl; 115 | 116 | // td::Plane normalV; 117 | // seg.singleFitting(td::Segmentation::RANSAC, normalV); 118 | 119 | // double n1[3]; 120 | // n1[0] = normalV.getA(); 121 | // n1[1] = normalV.getB(); 122 | // n1[2] = normalV.getC(); 123 | // std::vector pls; 124 | // pls = seg.getPlaneModels(); 125 | // std::vector indice = seg.getIndices(); 126 | 127 | // int tagetNum = 0; 128 | // for(int i = 0; i < pls.size(); i++) 129 | // { 130 | 131 | // if(indice[i].size()< minN) 132 | // continue; 133 | // double n2[3]; 134 | // n2[0] = pls[i].getA(); 135 | // n2[1] = pls[i].getB(); 136 | // n2[3] = pls[i].getC(); 137 | // double theta(0); 138 | // theta = n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]; 139 | // if(theta < 1.0) 140 | // tagetNum++; 141 | // } 142 | // std::cout << "共有 " << tagetNum << "块挡板" << std::endl; 143 | 144 | std::cout << "输入存储点云文件路径（不能带有空格）" << std::endl; 145 | std::cin >> path; 146 | seg.outPut(path); 147 | return 0; 148 | } 149 | --------------------------------------------------------------------------------