├── ICP ├── CMakeLists.txt ├── CourseWork1_BUFFIER.pdf ├── cloudManager.cpp ├── cloudManager.h ├── decoratedCloud.cpp ├── decoratedCloud.h └── main.cpp ├── RANSAC ├── include │ ├── cloudManager.h │ ├── decoratedCloud.h │ ├── evaluation.h │ ├── gestion.h │ ├── primitive.h │ ├── primitiveManager.h │ ├── ransac.h │ ├── reconstruction.h │ └── typedefs.h ├── project_BUFFIER.pdf └── src │ ├── cloudManager.cpp │ ├── decoratedCloud.cpp │ ├── evaluation.cpp │ ├── gestion.cpp │ ├── main.cpp │ ├── plane.cpp │ ├── primitiveManager.cpp │ ├── ransac.cpp │ ├── reconstruction.cpp │ └── sphere.cpp ├── README.md └── Smoothing ├── CMakeLists.txt ├── coursework2.pdf ├── include └── acq │ ├── cloudManager.h │ ├── decoratedCloud.h │ ├── discreteCurvature.h │ ├── impl │ ├── cloudManager.hpp │ ├── decoratedCloud.hpp │ ├── discreteCurvature.hpp │ ├── normalEstimation.hpp │ └── smoothing.hpp │ ├── normalEstimation.h │ ├── smoothing.h │ └── typedefs.h └── src ├── cloudManager.cpp ├── decoratedCloud.cpp ├── discreteCurvature.cpp ├── main.cpp ├── normalEstimation.cpp └── smoothing.cpp /ICP/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(IGLFramework) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | 6 | # Compile type: Release, Debug, RelWithDebInfo 7 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 8 | 9 | # Where external libraries are 10 | set(THIRD_PARTY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty) 11 | 12 | # definitions 13 | #add_definitions(-DIGL_VIEWER_WITH_NANOGUI -DNANOVG_GL3_IMPLEMENTATION) 14 | add_definitions(-DIGL_VIEWER_WITH_NANOGUI) 15 | 16 | 17 | # ################################################################ # 18 | # IGL 19 | # ################################################################ # 20 | 21 | include_directories(${THIRD_PARTY_DIR}/libigl/include) 22 | 23 | #include_directories(${THIRD_PARTY_DIR}/libigl/install/include) 24 | #link_directories(${THIRD_PARTY_DIR}/libigl/lib) 25 | 26 | # ################################################################ # 27 | # NanoGui 28 | # ################################################################ # 29 | 30 | # Disable building extras we won't need (pure C++ project) 31 | set(NANOGUI_BUILD_EXAMPLE OFF CACHE BOOL " " FORCE) 32 | set(NANOGUI_BUILD_PYTHON OFF CACHE BOOL " " FORCE) 33 | set(NANOGUI_INSTALL OFF CACHE BOOL " " FORCE) 34 | #set(NANOGUI_USE_GLAD OFF CACHE BOOL " " FORCE) 35 | #set(NANOGUI_USE_GLAD_DEFAULT OFF CACHE BOOL " " FORCE) 36 | 37 | # Add the configurations from nanogui 38 | add_subdirectory(${THIRD_PARTY_DIR}/libigl/external/nanogui) 39 | if (NOT WIN32) 40 | set_target_properties(nanogui-obj PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 41 | set_target_properties(nanogui PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 42 | endif() 43 | 44 | include_directories(${THIRD_PARTY_DIR}/libigl/external/nanogui/include) 45 | 46 | # For reliability of parallel build, make the NanoGUI targets dependencies 47 | set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies") 48 | 49 | # Various preprocessor definitions have been generated by NanoGUI 50 | add_definitions(${NANOGUI_EXTRA_DEFS}) 51 | MESSAGE(STATUS "NANOGUI_EXTRA_DEFS ${NANOGUI_EXTRA_DEFS}") 52 | # prints: -DNANOGUI_SHARED;-DNVG_SHARED;-DGLAD_GLAPI_EXPORT 53 | 54 | # On top of adding the path to nanogui/include, you may need extras 55 | include_directories(${NANOGUI_EXTRA_INCS}) 56 | MESSAGE(STATUS "NANOGUI_EXTRA_INCS: ${NANOGUI_EXTRA_INCS}") 57 | # prints: \ 58 | # 3rdparty/libigl/external/nanogui/ext/glfw/include; \ 59 | # 3rdparty/libigl/external/nanogui/ext/nanovg/src; \ 60 | # 3rdparty/libigl/external/nanogui/ext/eigen 61 | 62 | MESSAGE(STATUS "NANOGUI_EXTRA_LIBS: ${NANOGUI_EXTRA_LIBS}") 63 | # prints: GL;Xxf86vm;Xrandr;Xinerama;Xcursor;Xi;X11;pthread;rt;dl 64 | 65 | # ################################################################ # 66 | # Eigen 67 | # ################################################################ # 68 | 69 | if (DEFINED Eigen3_DIR) 70 | # use system version 71 | set(Eigen3_DIR ${THIRD_PARTY_DIR}/eigen) 72 | MESSAGE(STATUS "Eigen3_DIR: ${Eigen3_DIR}") 73 | find_package(Eigen3 REQUIRED HINTS ${Eigen3_DIR}) 74 | else() 75 | # use libigl's version 76 | set(EIGEN3_INCLUDE_DIR ${THIRD_PARTY_DIR}/libigl/external/nanogui/ext/eigen) 77 | endif() 78 | include_directories(${EIGEN3_INCLUDE_DIR}) 79 | MESSAGE(STATUS "EIGEN3_INCLUDE_DIR: ${EIGEN3_INCLUDE_DIR}") 80 | 81 | # ################################################################ # 82 | # NanoFLANN 83 | # ################################################################ # 84 | include_directories(${THIRD_PARTY_DIR}/nanoflann/include) 85 | 86 | # ################################################################ # 87 | # GLEW 88 | # ################################################################ # 89 | 90 | #if (WIN32) 91 | # set(GLEW_DIR "${THIRD_PARTY_DIR}/glew-2.0.0") 92 | # if (NOT EXISTS GLEW_DIR) 93 | # MESSAGE(FATAL Glew directory does not exist: ${GLEW_DIR}) 94 | # endif() 95 | # set(GLEW_LIBRARIES "glew32s") 96 | # set(GLEW_INCLUDE_DIRS "${GLEW_DIR}/include") 97 | # link_libraries(${GLEW_DIR}/lib/Release/x64) 98 | #else() 99 | if (NOT WIN32) 100 | set(GLEW_STATIC) 101 | find_package(GLEW REQUIRED) 102 | if (NOT DEFINED GLEW_LIBRARIES) 103 | MESSAGE(FATAL "GLEW LIBRARIES variable not filled...") 104 | endif() 105 | endif() 106 | include_directories(${GLEW_INCLUDE_DIRS}) 107 | 108 | 109 | # ################################################################ # 110 | # CGAL 111 | # ################################################################ # 112 | 113 | set(CMAKE_BUILD_TYPE Release) 114 | set(CGAL_DIR /usr/local/Cellar/CGAL-4.9) 115 | find_package(CGAL REQUIRED) 116 | include(${CGAL_USE_FILE}) 117 | 118 | # ################################################################ # 119 | # Project 120 | # ################################################################ # 121 | 122 | include_directories(include) 123 | 124 | # List of source files 125 | set(SOURCE_FILES 126 | src/main.cpp 127 | include/acq/typedefs.h 128 | include/acq/normalEstimation.h 129 | include/acq/impl/normalEstimation.hpp 130 | include/ANN/ANN.h 131 | include/ANN/ANNperf.h 132 | include/ANN/ANNx.h 133 | src/ANNsrc/bd_tree.h 134 | src/ANNsrc/kd_tree.h 135 | src/ANNsrc/kd_split.h 136 | src/ANNsrc/kd_search.h 137 | src/ANNsrc/kd_pr_search.h 138 | src/ANNsrc/kd_util.h 139 | src/ANNsrc/kd_fix_rad_search.h 140 | src/ANNsrc/pr_queue.h 141 | src/ANNsrc/pr_queue_k.h 142 | src/normalEstimation.cpp include/acq/impl/decoratedCloud.hpp include/acq/decoratedCloud.h include/acq/cloudManager.h include/acq/impl/cloudManager.hpp 143 | src/decoratedCloud.cpp src/cloudManager.cpp 144 | src/ANNsrc/ANN.cpp 145 | src/ANNsrc/bd_fix_rad_search.cpp 146 | src/ANNsrc/bd_pr_search.cpp 147 | src/ANNsrc/bd_search.cpp 148 | src/ANNsrc/bd_tree.cpp 149 | src/ANNsrc/brute.cpp 150 | src/ANNsrc/kd_dump.cpp 151 | src/ANNsrc/kd_tree.cpp 152 | src/ANNsrc/kd_split.cpp 153 | src/ANNsrc/kd_search.cpp 154 | src/ANNsrc/kd_pr_search.cpp 155 | src/ANNsrc/kd_util.cpp 156 | src/ANNsrc/kd_fix_rad_search.cpp 157 | src/ANNsrc/perf.cpp 158 | ) 159 | 160 | # Create program 161 | add_executable(iglFramework ${SOURCE_FILES}) 162 | 163 | # Add defines 164 | target_compile_definitions(iglFramework PUBLIC -DNANOVG_GL3_IMPLEMENTATION) 165 | if (WIN32) 166 | target_compile_definitions(iglFramework PUBLIC -DGLFW_INCLUDE_NONE -DNDEBUG -D_CONSOLE -D_USE_MATH_DEFINES -D_CRT_SECURE_NO_WARNINGS -DGLEW_STATIC) 167 | endif() 168 | 169 | if (NOT WIN32) 170 | set_target_properties(iglFramework PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 171 | endif() 172 | 173 | # Link dependency libraries 174 | target_link_libraries(iglFramework 175 | nanogui 176 | ${NANOGUI_EXTRA_LIBS} 177 | ${GLEW_LIBRARIES} 178 | ${CGAL_LIBRARIES} 179 | ) 180 | 181 | if (WIN32) 182 | add_custom_command(TARGET iglFramework POST_BUILD 183 | COMMAND ${CMAKE_COMMAND} -E 184 | copy_if_different "${CMAKE_CURRENT_BINARY_DIR}/3rdparty/libigl/external/nanogui/${CMAKE_BUILD_TYPE}/nanogui.dll" 185 | $/nanogui.dll) 186 | endif() 187 | 188 | # Untested! 189 | # Optional to compile IGL: http://libigl.github.io/libigl/optional/ 190 | # cd libigl 191 | # mkdir lib 192 | # cd lib 193 | # cmake -DCMAKE_BUILD_TYPE=Release -DLIBIGL_WITH_NANOGUI=YES -DLIBIGL_WITH_ANTTWEAKBAR=NO -DLIBIGL_WITH_CGAL=NO ../optional 194 | 195 | -------------------------------------------------------------------------------- /ICP/CourseWork1_BUFFIER.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mbuffier/3D_Geometry_Processing/61807fdd0962addcd1907c246e9225876a5292c5/ICP/CourseWork1_BUFFIER.pdf -------------------------------------------------------------------------------- /ICP/cloudManager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/cloudManager.hpp" 6 | #include 7 | 8 | namespace acq { 9 | 10 | void CloudManager::addCloud(DecoratedCloud const& cloud) { 11 | _clouds.push_back(cloud); 12 | } //...CloudManager::addCloud() 13 | 14 | void CloudManager::setCloud(DecoratedCloud const& cloud, int index) { 15 | if (index >= _clouds.size()) { 16 | if (index != _clouds.size()) 17 | std::cerr << "[CloudManager::setCloud] " 18 | << "Warning, creating " << index - _clouds.size() 19 | << " empty clouds when inserting to index " << index 20 | << ", current size is " << _clouds.size() 21 | << "...why not use addCloud?\n"; 22 | _clouds.resize(index + 1); 23 | } 24 | 25 | _clouds.at(index) = cloud; 26 | } //...CloudManager::setCloud() 27 | 28 | DecoratedCloud& CloudManager::getCloud(int index) { 29 | if (index < _clouds.size()) 30 | return _clouds.at(index); 31 | else { 32 | std::cerr << "Cannot return cloud with id " << index 33 | << ", only have " << _clouds.size() 34 | << " clouds...returning empty cloud\n"; 35 | throw new std::runtime_error("No such cloud"); 36 | } 37 | } //...CloudManager::getCloud() 38 | 39 | DecoratedCloud const& CloudManager::getCloud(int index) const { 40 | return const_cast( 41 | const_cast(this)->getCloud(index) 42 | ); 43 | } //...CloudManager::getCloud() (const) 44 | 45 | } //...ns acq -------------------------------------------------------------------------------- /ICP/cloudManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_CLOUDMANAGER_H 6 | #define ACQ_CLOUDMANAGER_H 7 | 8 | #include "acq/decoratedCloud.h" 9 | #include 10 | #include 11 | 12 | namespace acq { 13 | 14 | /** \brief Small class to keep track of multiple point clouds. */ 15 | class CloudManager { 16 | public: 17 | 18 | /** \brief Append a cloud to the list of clouds. */ 19 | void addCloud(DecoratedCloud const& cloud); 20 | 21 | int size() {return _clouds.size() ;} 22 | 23 | /** \brief Overwrite a cloud at a specific index. */ 24 | void setCloud(DecoratedCloud const& cloud, int index); 25 | 26 | /** \brief Get cloud with specific index. */ 27 | DecoratedCloud& getCloud(int index); 28 | 29 | /** \brief Get cloud with specific index (const version). */ 30 | DecoratedCloud const& getCloud(int index) const; 31 | 32 | protected: 33 | std::vector _clouds; //!< List of clouds possibly with normals and faces. 34 | public: 35 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | }; //...class CloudManager 38 | 39 | } //...ns acq 40 | 41 | #endif // ACQ_CLOUDMANAGER_H 42 | -------------------------------------------------------------------------------- /ICP/decoratedCloud.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/decoratedCloud.hpp" 6 | //#include namespace std; 7 | namespace acq { 8 | 9 | /*** Old constructors ***/ 10 | DecoratedCloud::DecoratedCloud(CloudT const& vertices) 11 | : _vertices(vertices) {} 12 | 13 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces) 14 | : _vertices(vertices), _faces(faces) 15 | {} 16 | 17 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals) 18 | : _vertices(vertices), _faces(faces), _normals(normals) 19 | {} 20 | 21 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, NormalsT const& normals) 22 | : _vertices(vertices), _normals(normals) 23 | {} 24 | 25 | 26 | /* ********* New constructors : from 2 and 6 meshes ************ */ 27 | DecoratedCloud::DecoratedCloud(DecoratedCloud const& cloud1, DecoratedCloud const& cloud2, bool overlap) 28 | { 29 | // Concatenate (V1,F1, N1) and (V2,F2, N2) into (V,F, N) 30 | Eigen::MatrixXd const &V1 = cloud1.getVertices() ; 31 | Eigen::MatrixXd const &V2 = cloud2.getVertices(); 32 | 33 | Eigen::MatrixXi const &F1 = cloud1.getFaces() ; 34 | Eigen::MatrixXi const &F2 = cloud2.getFaces() ; 35 | 36 | Eigen::MatrixXd const &N1 = cloud1.getNormals() ; 37 | Eigen::MatrixXd const &N2 = cloud2.getNormals() ; 38 | 39 | // compute new vertices matrix 40 | Eigen::MatrixXd V(V1.rows()+V2.rows(),V1.cols()); 41 | V< T) { 235 | // construct the transformation matrix 236 | Eigen::Matrix4d transform = Eigen::MatrixXd::Zero(4,4) ; 237 | transform.topLeftCorner(3, 3) = R ; 238 | transform.topRightCorner(3, 1) = T ; 239 | transform.bottomLeftCorner(1, 3) = Eigen::MatrixXd::Zero(1,3) ; 240 | transform(3,3) =1 ; 241 | 242 | // transform coordinates in homogenous coordinates 243 | Eigen::MatrixXd homogenousVert(_vertices.rows(),_vertices.cols()+1) ; 244 | homogenousVert.leftCols(3) = _vertices ; 245 | homogenousVert.col(3) = Eigen::MatrixXd::Ones(_vertices.rows(),1) ; 246 | 247 | // apply the transformation and gives back the transformated points 248 | Eigen::MatrixXd newVertices(_vertices.cols()+1,_vertices.rows()) ; 249 | newVertices = transform*homogenousVert.transpose() ; 250 | 251 | // move the mesh 252 | _vertices = newVertices.transpose().topLeftCorner(_vertices.rows(), _vertices.cols()) ; 253 | } 254 | 255 | // move a mesh with an angle theta around y and a translation (x,y,z) 256 | void DecoratedCloud::install(double theta, float x, float y, float z) { 257 | // rotation 258 | Eigen::Matrix3d R ; 259 | R << cos(theta), 0, sin(theta), 260 | 0, 1, 0, 261 | -sin(theta), 0, cos(theta) ; 262 | 263 | // translation matrix 264 | Eigen::Matrix T ; 265 | T << x,y,z ; 266 | 267 | // transform the mesh 268 | this->transformation(R,T) ; 269 | } 270 | 271 | /* ********* Functions to add noise ************ */ 272 | 273 | // compute the bounding box of a mesh 274 | void DecoratedCloud::boundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin) { 275 | Eigen::MatrixXd maximum(1, 3) ; 276 | maximum = _vertices.colwise().maxCoeff() ; 277 | Xmax = maximum(0) ; 278 | Ymax= maximum(1) ; 279 | Zmax = maximum(2) ; 280 | 281 | Eigen::MatrixXd minimum(1, 3) ; 282 | minimum = _vertices.colwise().minCoeff() ; 283 | Xmin = minimum(0) ; 284 | Ymin = minimum(1) ; 285 | Zmin = minimum(2) ; 286 | 287 | } 288 | 289 | // add noise 290 | void DecoratedCloud::addNoise(float sigmaX, float sigmaY, float sigmaZ) { 291 | int M = _vertices.size() ; 292 | 293 | // initialization 294 | Eigen::MatrixXd random(M, 3) ; 295 | 296 | // construct noise 297 | for(int i = 0; i< M; i++) { 298 | random(i, 0) = std::rand()*sigmaX /RAND_MAX ; 299 | random(i, 1) = std::rand()*sigmaY /RAND_MAX ; 300 | random(i, 2) = std::rand()*sigmaZ /RAND_MAX ; 301 | } 302 | // add noise to the vertices 303 | _vertices += random ; 304 | } 305 | 306 | /* ********* ICP function ************ */ 307 | 308 | void DecoratedCloud::icpAlgo(DecoratedCloud const& cloud,int const nbP, ANNkd_tree* kdTreeConst) { 309 | 310 | // get back the dimensions of the matrix 311 | int nPts = cloud.getVertices().rows() ; 312 | int dim = cloud.getVertices().cols() ; 313 | int M = _vertices.rows() ; 314 | 315 | // sample the indice for the given number of points 316 | Eigen::VectorXi sampleIndices(nbP, 1);// indices sample 317 | sampleIndices = sample(nbP, M) ; 318 | 319 | // create the array for the kd-tree points to sample 320 | ANNpointArray sampleVertices ; // array of points 321 | sampleVertices = annAllocPts(nbP, dim); 322 | ANNpointArray sampleNormals ; // array of normals 323 | sampleNormals = annAllocPts(nbP, dim); 324 | 325 | // sample the cloud which move 326 | sampleVertices = this->sampleVertex(sampleIndices) ; 327 | sampleNormals = this->sampleNormal(sampleIndices) ; 328 | 329 | // initialization for the search into the kd-tree 330 | ANNpoint queryPt; 331 | queryPt = annAllocPt(dim); 332 | 333 | // to check the normals : point from the fixed cloud 334 | Eigen::Matrix normalPt ; 335 | Eigen::Matrix verticeNormal ; 336 | 337 | // to store the result 338 | Eigen::MatrixXd resultpoint(nbP, 3) ; 339 | Eigen::MatrixXd resultmatching(nbP, 3) ; 340 | 341 | int index = 0 ; 342 | // fill the neighbor distance and indices 343 | for (int i=0; iannkSearch( // search 352 | queryPt, // query point 353 | 1, // number of near neighbors 354 | nnIdx, // nearest neighbors (returned) 355 | dists, // distance (returned) 356 | 0.0); // error bound 357 | 358 | // take the result 359 | int indexFind = nnIdx[0] ; 360 | double distance = dists[0] ; 361 | 362 | // to test the dot product, transformation into a vector 363 | for(int j=0 ; j<3 ; j++) { 364 | normalPt(0,j) = sampleNormals[i][j] ; 365 | } 366 | 367 | verticeNormal = cloud.getNormals().row(indexFind) ; 368 | 369 | // compute the dot product 370 | float test = std::abs(normalPt.dot(verticeNormal)) ; 371 | 372 | // test to elimanate bad matches 373 | if (distance < 0.001 && test > 0.98 ) { 374 | for(int j=0 ; j<3 ; j++) { 375 | resultpoint(index,j) = sampleVertices[i][j] ; 376 | } 377 | 378 | resultmatching.row(index) = cloud.getVertices().row(indexFind) ; 379 | index +=1 ; 380 | } 381 | // free memory 382 | delete [] nnIdx ; 383 | delete [] dists; 384 | } 385 | // free memory 386 | annDeallocPts(sampleVertices) ; 387 | annDeallocPts(sampleNormals) ; 388 | annDeallocPt(queryPt) ; 389 | 390 | // print the result for this search 391 | float pour = (float(index)/nbP)*100.0 ; 392 | std::cout<< "Number of points used : " << index << " over " << nbP << " points sampled (" << pour <<"%)"<< std::endl ; 393 | 394 | index -= 1 ; 395 | Eigen::MatrixXd resultPoint(index, 3) ; 396 | Eigen::MatrixXd resultMatching(index, 3) ; 397 | 398 | // only take the points which matched 399 | resultPoint = resultpoint.topRows(index) ; 400 | resultMatching = resultmatching.topRows(index) ; 401 | 402 | // barycenter for the moving cloud 403 | Eigen::Matrix barySampleP ; 404 | barySampleP = resultPoint.colwise().sum()/index ; 405 | Eigen::MatrixXd barySamplePArray(index, 3) ; 406 | barySamplePArray = barySampleP.replicate(index,1) ; 407 | 408 | // barycenter for the fixed cloud 409 | Eigen::Matrix baryCouldP ; 410 | baryCouldP = resultMatching.colwise().sum()/index ; 411 | Eigen::MatrixXd baryCouldPArray(index, 3) ; 412 | baryCouldPArray = baryCouldP.replicate(index,1) ; 413 | 414 | // centered points for moving cloud 415 | resultPoint = resultPoint-barySamplePArray ; 416 | 417 | // centered points for fixed cloud 418 | resultMatching = resultMatching - baryCouldPArray ; 419 | 420 | // creation of R 421 | Eigen::Matrix3d A = (resultMatching.transpose())*resultPoint ; 422 | Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); 423 | Eigen::Matrix3d U = svd.matrixU() ; 424 | Eigen::Matrix3d V = svd.matrixV() ; 425 | Eigen::Matrix3d R ; 426 | R = U*(V.transpose()) ; 427 | 428 | // creation of T 429 | Eigen::Matrix T ; 430 | T = baryCouldP.transpose() - R*barySampleP.transpose() ; 431 | 432 | // transformation of the cloud 433 | this->transformation(R,T) ; 434 | } 435 | 436 | 437 | // apply ICP point to plane 438 | void DecoratedCloud::icpPointToPlane(DecoratedCloud const& cloud,int const nbP, ANNkd_tree* kdTreeConst) { 439 | 440 | // dimensions of the matrix 441 | int nPts = cloud.getVertices().rows() ; 442 | int dim = cloud.getVertices().cols() ; 443 | int M = _vertices.rows() ; 444 | 445 | // sample nbp points from the matrix 446 | Eigen::VectorXi sampleIndices(nbP, 1);// indices sample 447 | sampleIndices = sample(nbP, M) ; 448 | 449 | // create the array for the kd-tree points to sample 450 | ANNpointArray sampleVertices ; // array of points 451 | sampleVertices = annAllocPts(nbP, dim); 452 | ANNpointArray sampleNormals ; // array of normals 453 | sampleNormals = annAllocPts(nbP, dim); 454 | 455 | sampleVertices = this->sampleVertex(sampleIndices) ; 456 | sampleNormals = this->sampleNormal(sampleIndices) ; 457 | 458 | // initialization 459 | ANNpoint queryPt; 460 | queryPt = annAllocPt(dim); 461 | 462 | // initialization to fill A 463 | Eigen::Matrix normalPt ; 464 | Eigen::Matrix coordPt ; 465 | Eigen::Matrix coordPtFixed ; 466 | Eigen::Matrix normalPtFixed ; 467 | 468 | // to solve the system 469 | Eigen::MatrixXd A(nbP, 6) ; 470 | Eigen::MatrixXd b(nbP, 1) ; 471 | 472 | int index = 0 ; 473 | // fill the neighbor distance and indices 474 | for (int i=0; iannkSearch( // search 484 | queryPt, // query point 485 | 1, // number of near neighbors 486 | nnIdx, // nearest neighbors (returned) 487 | dists, // distance (returned) 488 | 0.0); // error bound 489 | 490 | // take the result 491 | int indexFind = nnIdx[0] ; 492 | double distance = dists[0] ; 493 | 494 | // transformation into a matrix for algebric computation 495 | for(int j=0 ; j<3 ; j++) { 496 | normalPt(0,j) = sampleNormals[i][j] ; 497 | coordPt(0,j) = sampleVertices[i][j] ; 498 | } 499 | coordPtFixed = cloud.getVertices().row(indexFind) ; 500 | normalPtFixed = cloud.getNormals().row(indexFind) ; 501 | 502 | // compute the dot product 503 | float test = std::abs(normalPt.dot(normalPtFixed)) ; 504 | 505 | // test to elimanate bad matches 506 | if (distance < 0.001 && test > 0.5 ) { 507 | // fill A matrix 508 | A.row(index) << coordPt.cross(normalPt), normalPt ; 509 | 510 | // fill b vector 511 | b(index) = -normalPt.dot(coordPt-coordPtFixed) ; 512 | index +=1 ; 513 | } 514 | 515 | // free memory 516 | delete [] nnIdx ; 517 | delete [] dists; 518 | } 519 | // free memory 520 | annDeallocPts(sampleVertices) ; 521 | annDeallocPts(sampleNormals) ; 522 | annDeallocPt(queryPt) ; 523 | 524 | // print the result for this search 525 | float pour = (float(index)/nbP)*100.0 ; 526 | std::cout<< "Number of points used : " << index << " over " << nbP << " points sampled (" << pour <<"%)"<< std::endl ; 527 | 528 | // only take the points which matched 529 | index -= 1 ; 530 | A = A.topRows(index) ; 531 | b = b.topRows(index) ; 532 | 533 | // save Ax = b to find R and T 534 | Eigen::VectorXd result = A.colPivHouseholderQr().solve(b); 535 | 536 | // construct the rotation from the resulting vector 537 | Eigen::Matrix3d R ; 538 | R = constructR(result(0), result(1), result(2)) ; 539 | 540 | Eigen::Matrix T ; 541 | T << result(3), result(4), result(5) ; 542 | 543 | // apply the transformation 544 | this->transformation(R,T) ; 545 | } 546 | 547 | } //...ns acq 548 | 549 | -------------------------------------------------------------------------------- /ICP/decoratedCloud.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_DECORATEDCLOUD_H 6 | #define ACQ_DECORATEDCLOUD_H 7 | 8 | #include "acq/typedefs.h" 9 | #include 10 | #include // ANN declarations 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "igl/copyleft/cgal/intersect_other.h" 17 | #include "acq/impl/normalEstimation.hpp" // Templated functions 18 | #include 19 | 20 | namespace acq { 21 | 22 | /** \brief Simple class to keep track of points normals and faces for a point cloud or mesh. */ 23 | class DecoratedCloud { 24 | public: 25 | /**Old Constructors */ 26 | explicit DecoratedCloud() {} 27 | explicit DecoratedCloud(CloudT const& vertices); 28 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces); 29 | explicit DecoratedCloud(CloudT const& vertices, NormalsT const& normals); 30 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals); 31 | 32 | /** new Constructors to obtain a cloud from several to visualize (with a boolean to show overlap) */ 33 | explicit DecoratedCloud(DecoratedCloud const& cloud1, DecoratedCloud const& cloud2, bool overlap) ; 34 | 35 | explicit DecoratedCloud(DecoratedCloud const& cloud1, DecoratedCloud const& cloud2, 36 | DecoratedCloud const& cloud3, DecoratedCloud const& cloud4, 37 | DecoratedCloud const& cloud5, DecoratedCloud const& cloud6) ; 38 | 39 | /** Old getters/seters */ 40 | CloudT const& getVertices() const { return _vertices; } 41 | void setVertices(CloudT const& vertices) { _vertices = vertices; } 42 | bool hasVertices() const { return static_cast(_vertices.size()); } 43 | 44 | FacesT const& getFaces() const { return _faces;} 45 | void setFaces(FacesT const& faces) { _faces = faces; } 46 | bool hasFaces() const { return static_cast(_faces.size()); } 47 | 48 | NormalsT & getNormals() { return _normals; } 49 | NormalsT const& getNormals() const { return _normals; } 50 | void setNormals(NormalsT const& normals) { _normals = normals; } 51 | bool hasNormals() const { return static_cast(_normals.size()); } 52 | 53 | /** New getter for the color */ 54 | ColorsT const& getColors() const { return _color;} 55 | 56 | /** Friend functions */ 57 | // transform an array to a matrix 58 | friend Eigen::MatrixXd arrayToMatrix(ANNpointArray arrayPoint, int size); 59 | // sample numberPoint integer from a sizeMatrix sized matrix 60 | friend Eigen::VectorXi sample(int const numberPoint, int sizeMatrix) ; 61 | // give back a rotation matrix from 3 angles on principal axis 62 | friend Eigen::Matrix3d constructR(float rx, float ry, float rz) ; 63 | 64 | /** Functions to sample the clouds */ 65 | // give back an array of vertex from a vector of indices 66 | ANNpointArray sampleVertex(Eigen::VectorXi sampleIndices) const ; 67 | // same for normals 68 | ANNpointArray sampleNormal(Eigen::VectorXi sampleIndices ) const ; 69 | 70 | /** Functions to move meshes */ 71 | // move a mesh from a rotation matrix T and a translation vector 72 | void transformation( Eigen::Matrix3d R, Eigen::Matrix T) ; 73 | // move a mesh with a rotation theta around y and a translation(x,y,z) 74 | void install(double theta, float x, float y, float z) ; 75 | 76 | /** Functions to add noise */ 77 | // compute the bounding box 78 | void boundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin); 79 | // add noise to a mesh according to standart deviations computed from the bounding box 80 | void addNoise(float sigmaX, float sigmaY, float sigmaZ) ; 81 | 82 | /** ICP function */ 83 | void icpAlgo(DecoratedCloud const& cloud, int const nPts, ANNkd_tree* kdTreeConst) ; 84 | void icpPointToPlane(DecoratedCloud const& cloud, int const nPts, ANNkd_tree* kdTreeConst) ; 85 | 86 | protected: 87 | CloudT _vertices; //!< Point cloud, N x 3 matrix where N is the number of points. 88 | FacesT _faces; //!< Faces stored as rows of vertex indices (referring to \ref _vertices). 89 | NormalsT _normals; //!< Per-vertex normals, associated with \ref _vertices by row ID. 90 | ColorsT _color ; 91 | public: 92 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 94 | }; //...class DecoratedCloud() 95 | 96 | } //...ns acq 97 | 98 | #endif //ACQ_DECORATEDCLOUD_H -------------------------------------------------------------------------------- /RANSAC/include/cloudManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_CLOUDMANAGER_H 6 | #define ACQ_CLOUDMANAGER_H 7 | 8 | #include "acq/decoratedCloud.h" 9 | #include 10 | 11 | namespace acq { 12 | 13 | /** \brief Small class to keep track of multiple point clouds. */ 14 | class CloudManager { 15 | public: 16 | /** \brief Append a cloud to the list of clouds. */ 17 | void addCloud(DecoratedCloud const& cloud); 18 | 19 | /** \brief Overwrite a cloud at a specific index. */ 20 | void setCloud(DecoratedCloud const& cloud, int index); 21 | 22 | /** \brief Get cloud with specific index. */ 23 | DecoratedCloud& getCloud(int index); 24 | 25 | /** \brief Get cloud with specific index (const version). */ 26 | DecoratedCloud const& getCloud(int index) const; 27 | 28 | /** \brief Delete cloud with specific index (const version). */ 29 | void deleteCloud(int index); 30 | 31 | // clear all the cloud in the cloudManager 32 | void clearCloud() ; 33 | 34 | // get cloud size 35 | int getCloudSize(){return _clouds.size();}; 36 | 37 | // delete the primitives from the given index 38 | void deleteCloudFromIndex(int indexStart); 39 | 40 | protected: 41 | std::vector _clouds; //!< List of clouds possibly with normals and faces. 42 | 43 | public: 44 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | }; //...class CloudManager 47 | 48 | } //...ns acq 49 | 50 | #endif // ACQ_CLOUDMANAGER_H 51 | -------------------------------------------------------------------------------- /RANSAC/include/decoratedCloud.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_DECORATEDCLOUD_H 6 | #define ACQ_DECORATEDCLOUD_H 7 | 8 | #include "acq/typedefs.h" 9 | 10 | namespace acq { 11 | 12 | /** \brief Simple class to keep track of points normals and faces for a point cloud or mesh. */ 13 | class DecoratedCloud { 14 | public: 15 | /** \brief Default constructor leaving fields empty. */ 16 | explicit DecoratedCloud() {} 17 | 18 | // destructeur for get cloud 19 | ~DecoratedCloud() {} 20 | 21 | /** \brief Constructor filling point information only. */ 22 | explicit DecoratedCloud(CloudT const& vertices); 23 | 24 | /** \brief Constructor filling point and face information. */ 25 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces); 26 | 27 | /** \brief Constructor filling point and normal information. */ 28 | explicit DecoratedCloud(CloudT const& vertices, NormalsT const& normals); 29 | 30 | /** \brief Constructor filling point and normal information. */ 31 | explicit DecoratedCloud(CloudT const& vertices, NormalsT const& normals, ColorT const& colors); 32 | 33 | /** \brief Constructor filling point, face and normal information. */ 34 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals); 35 | 36 | /** \brief Constructor filling point, face and normal information. */ 37 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals, ColorT const& colors); 38 | 39 | /** \brief Getter for point cloud. */ 40 | CloudT const& getVertices() const { return _vertices; } 41 | /** \brief Setter for point cloud. */ 42 | void setVertices(CloudT const& vertices) { _vertices = vertices; } 43 | /** \brief Check, if any points stored. */ 44 | bool hasVertices() const { return static_cast(_vertices.size()); } 45 | 46 | /** \brief Getter for face indices list. */ 47 | FacesT const& getFaces() const { return _faces; } 48 | /** \brief Setter for face indices list. */ 49 | void setFaces(FacesT const& faces) { _faces = faces; } 50 | /** \brief Check, if any faces stored. */ 51 | bool hasFaces() const { return static_cast(_faces.size()); } 52 | 53 | /** \brief Getter for normals. */ 54 | NormalsT & getNormals() { return _normals; } 55 | /** \brief Getter for normals (const version). */ 56 | NormalsT const& getNormals() const { return _normals; } 57 | /** \brief Setter for normals. */ 58 | void setNormals(NormalsT const& normals) { _normals = normals; } 59 | /** \brief Check, if any normals stored. */ 60 | bool hasNormals() const { return static_cast(_normals.size()); } 61 | 62 | 63 | /** \brief Getter for point cloud. */ 64 | ColorT const& getColors() const { return _colors; } 65 | /** \brief Setter for point cloud. */ 66 | void setColors(ColorT const& colors) { _colors = colors; } 67 | 68 | 69 | 70 | protected: 71 | CloudT _vertices; //!< Point cloud, N x 3 matrix where N is the number of points. 72 | FacesT _faces; //!< Faces stored as rows of vertex indices (referring to \ref _vertices). 73 | NormalsT _normals; //!< Per-vertex normals, associated with \ref _vertices by row ID. 74 | ColorT _colors; 75 | public: 76 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | }; //...class DecoratedCloud() 79 | 80 | } //...ns acq 81 | 82 | #endif //ACQ_DECORATEDCLOUD_H 83 | -------------------------------------------------------------------------------- /RANSAC/include/evaluation.h: -------------------------------------------------------------------------------- 1 | #ifndef ACQ_EVALUATION_H 2 | #define ACQ_EVALUATION_H 3 | 4 | #include "acq/cloudManager.h" 5 | #include "acq/primitiveManager.h" 6 | 7 | #include // ANN declarations 8 | 9 | namespace acq { 10 | // ***********************$ Function to add noise and test smoothing ****************** 11 | void computeBoundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin, DecoratedCloud& cloud) ; 12 | 13 | // with 1 add noise on the vertices, with 2 add noise on the normals 14 | Eigen::MatrixXd addNoise(float noise, DecoratedCloud& cloud, int typeMatrix) ; 15 | 16 | 17 | // ***********************$ Function to perform connected component algorithm ****************** 18 | // apply the algorithm to each cloud the plane of a cloudmanager 19 | void connectedComponentManager(CloudManager& thisCloudManager, PrimitiveManager& best_primitives, double threshold) ; 20 | 21 | // recursive connected component algorithm to separate distant part of a cloud 22 | void connectedComponent(DecoratedCloud& cloud, double threshold) ; 23 | 24 | // label a given vertices if its neighbors aren't too far 25 | void labelVertices(Eigen::RowVector3d thisColor, ANNpointArray verticesArray, Eigen::MatrixXd& colors, 26 | int this_idx, Eigen::MatrixXd& visited, ANNkd_tree* kdTree, double threshold, int connectivity) ; 27 | 28 | // use to convert a matrix in an array for ANN librairy 29 | ANNpointArray matrixToANNArray(Eigen::MatrixXd const& points) ; 30 | } 31 | 32 | #endif 33 | 34 | // do connected component -------------------------------------------------------------------------------- /RANSAC/include/gestion.h: -------------------------------------------------------------------------------- 1 | #ifndef ACQ_GESTION_H 2 | #define ACQ_GESTION_H 3 | 4 | #include "acq/primitiveManager.h" 5 | #include "acq/cloudManager.h" 6 | #include "acq/typedefs.h" 7 | 8 | #include 9 | #include 10 | 11 | namespace acq { 12 | 13 | // ****** ============ Helper Functions =============== ******* 14 | Eigen::MatrixXi sample(int cloudSize) ; // sample the point cloud 15 | Eigen::Matrix3d computeVariance(Eigen::MatrixXd V); // compute the variance 16 | 17 | // ****** ============ Functions to handle a sphere =============== ******* 18 | // determine if the 3 points gives a sphere or no 19 | int isSphere(Eigen::Matrix3d vertices, Eigen::Matrix3d normals, double threshold, double alpha) ; 20 | 21 | // create and store the sphere when it exists 22 | void computeSphere(Eigen::Matrix sample_idx, Eigen::Matrix3d variance, DecoratedCloud& cloud, PrimitiveManager& primitives, double threshold, double alpha); 23 | 24 | // compute the attributs using as many points as we have 25 | Eigen::Matrix computerCenter(Eigen::MatrixXd vertices, Eigen::MatrixXd normals) ; 26 | double computerRadius(Eigen::MatrixXd thisVertices, Eigen::Matrix thisCenter) ; 27 | 28 | /********* ============= Functions to handle PLANE =============== *********/ 29 | void computePlane(Eigen::Matrix sample_idx, Eigen::Matrix3d variance, DecoratedCloud &cloud, PrimitiveManager& primitives, double thresh, double alpha); 30 | bool isPlane(Eigen::MatrixXd V, Eigen::MatrixXd N, double T, double alpha); 31 | Eigen::Matrix computeNormal(Eigen::MatrixXd V, Eigen::Matrix _N); 32 | 33 | /********* ============= Functions to handle the final cloud =============== *********/ 34 | void fuse(PrimitiveManager& best_primitives, CloudManager& clouds, double T_rad, double T_cent, double T_norm, double T_refPt); 35 | DecoratedCloud* gatherClouds(CloudManager& cloudManager,int colorExit) ; 36 | void cleanCloud(DecoratedCloud& cloudRansac, CloudManager& cloudManager, Eigen::MatrixXi inliers_idx) ; 37 | 38 | void reconstruct(PrimitiveManager& best_primitives, DecoratedCloud& cloud, int nbSamples, double T, double alpha, double T2); 39 | void sampleFromPrimitive(DecoratedCloud& cloud, Eigen::MatrixXi inliers_idx, Primitive* p, int nbSamples, double T); 40 | } 41 | 42 | #endif 43 | 44 | -------------------------------------------------------------------------------- /RANSAC/include/primitive.h: -------------------------------------------------------------------------------- 1 | #ifndef ACQ_PRIMITIVE_H 2 | #define ACQ_PRIMITIVE_H 3 | 4 | #include "acq/typedefs.h" 5 | #include "acq/decoratedCloud.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | namespace acq { 14 | 15 | /** ---- PRIMITIVE ---- */ 16 | class Primitive { 17 | 18 | public: 19 | // constructor/destructor 20 | Primitive() {} ; 21 | virtual ~Primitive() {} ; 22 | 23 | // getters.setters 24 | double getScore()const{return _score;} 25 | int getType()const{return _type;} 26 | Eigen::MatrixXi getInliers_idx()const{return _inliers_idx;} 27 | 28 | void setType(int type) {_type = type ; } 29 | void setScore(double score){_score = score;} 30 | void setInliers_idx(Eigen::MatrixXi inliers_idx){_inliers_idx = inliers_idx;} 31 | 32 | // Virtual Functions 33 | virtual double getRadius(){}; 34 | virtual Eigen::Matrix getCenter(){}; 35 | virtual Eigen::Matrix getNormal(){}; 36 | virtual Eigen::Matrix getRefPoint(){}; 37 | virtual Eigen::MatrixXi computeInliers(DecoratedCloud& cloud, double threshold, double alpha){}; 38 | virtual void computeScore(Eigen::Matrix3d variance, DecoratedCloud& pointCloud, double threshold, double alpha){}; 39 | virtual int findBestNumberPoints(Eigen::Matrix3d variance){}; 40 | virtual Primitive* clone(){}; 41 | 42 | protected: 43 | double _score; 44 | int _type; // 1: Sphere, 2: Plane 45 | Eigen::MatrixXi _inliers_idx; 46 | }; 47 | 48 | /** ---- SPHERE ---- */ 49 | class Sphere : public Primitive { 50 | 51 | public: 52 | // Constructor and Destructor 53 | Sphere(double radius, Eigen::Matrix center) : _radius(radius), _center(center) {} ; 54 | ~Sphere(){}; 55 | 56 | // Getters/Setters 57 | double getRadius() const {return _radius;} 58 | Eigen::Matrix getCenter()const{return _center;} 59 | void setRadius(double radius){_radius = radius;} 60 | void setCenter(Eigen::Matrix center){_center = center;} 61 | 62 | // functions specific to spheres 63 | void computeScore(Eigen::Matrix3d variance, DecoratedCloud& cloud, double threshold, double alpha); 64 | Eigen::MatrixXi computeInliers(DecoratedCloud& cloud, double threshold, double alpha); 65 | int findBestNumberPoints(Eigen::Matrix3d variance) ; 66 | Primitive* clone() ; 67 | 68 | protected: 69 | // radius and center of the sphere 70 | double _radius; 71 | Eigen::Matrix _center; 72 | }; 73 | 74 | /** ---- PLANE ---- */ 75 | class Plane : public Primitive { 76 | 77 | public: 78 | // Constructor and Destructor 79 | Plane(Eigen::Matrix refPoint, Eigen::Matrix normal) : _refPoint(refPoint), _normal(normal) {} ; 80 | ~Plane(){}; 81 | 82 | // getters/setters 83 | Eigen::Matrix getNormal() const {return _normal;} 84 | Eigen::Matrix getRefPoint()const {return _refPoint;} 85 | void setNormal(Eigen::Matrix normal) {_normal = normal;} 86 | void setRefPoint(Eigen::Matrix refPoint) {_refPoint = refPoint;} 87 | 88 | // this one is override because we use it one a primitive object 89 | Eigen::MatrixXi computeInliersPlane(DecoratedCloud& cloud, double threshold, double alpha) ; 90 | 91 | void computeScore(Eigen::Matrix3d variance, DecoratedCloud& cloud, double T, double alpha); 92 | Eigen::MatrixXi computeInliers(DecoratedCloud& cloud, double T, double alpha); 93 | int findBestNumberPoints(Eigen::Matrix3d var, DecoratedCloud& cloud,Eigen::MatrixXi inliers_idx); 94 | Primitive* clone() ; 95 | 96 | protected: 97 | Eigen::Matrix _refPoint; 98 | Eigen::Matrix _normal; 99 | }; 100 | } 101 | 102 | #endif 103 | -------------------------------------------------------------------------------- /RANSAC/include/primitiveManager.h: -------------------------------------------------------------------------------- 1 | #ifndef ACQ_PRIMITIVEMANAGER_H 2 | #define ACQ_PRIMITIVEMANAGER_H 3 | 4 | #include "acq/primitive.h" 5 | #include 6 | 7 | namespace acq { 8 | 9 | /** Small class to keep track of multiple primitives */ 10 | class PrimitiveManager { 11 | public: 12 | 13 | // test constructor 14 | PrimitiveManager() {} ; 15 | ~PrimitiveManager() {} ; 16 | 17 | /** add a primitive to the vector */ 18 | void addPrimitive(Primitive* primitive); 19 | 20 | /** set the primitive to a fixed place */ 21 | void setPrimitive(Primitive*, int index); 22 | 23 | /** get back the primitive from a vector */ 24 | Primitive* getPrimitive(int index); 25 | int getCloudSize() {return _primitives.size();}; 26 | 27 | // find the primitive with the best score : return the index 28 | int findBestScore() ; 29 | 30 | // delete primitive 31 | void deletePrimitive(int index) ; 32 | 33 | // clean all the primitive in the cloud 34 | void clearAllPrimitives() ; 35 | 36 | // delete from a given index to the end 37 | void deleteCloudFromIndex(int indexStart) ; 38 | protected: 39 | std::vector _primitives ; 40 | 41 | public: 42 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | }; 45 | 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /RANSAC/include/ransac.h: -------------------------------------------------------------------------------- 1 | #ifndef IGLFRAMEWORK_RANSAC_H 2 | #define IGLFRAMEWORK_RANSAC_H 3 | 4 | #include "acq/primitiveManager.h" 5 | #include "acq/decoratedCloud.h" 6 | #include "acq/gestion.h" 7 | #include "acq/cloudManager.h" 8 | 9 | namespace acq { 10 | // main function apply to a cloud which modifies best_primitives and cloudManager with the best primitives found 11 | bool ransac(DecoratedCloud& cloud, PrimitiveManager& best_primitives, CloudManager& cloudManager, 12 | double thresh, double alpha, double thresh_best, int iterationsTotal, int numberSample); 13 | } 14 | 15 | #endif 16 | 17 | -------------------------------------------------------------------------------- /RANSAC/include/reconstruction.h: -------------------------------------------------------------------------------- 1 | #ifndef ACQ_RECONSTRUCTION_H 2 | #define ACQ_RECONSTRUCTION_H 3 | 4 | #include "acq/cloudManager.h" 5 | #include "acq/primitiveManager.h" 6 | 7 | namespace acq { 8 | void reconstruct(PrimitiveManager &best_primitives, DecoratedCloud &cloud, int nbSamples, double T, double alpha, double T2); 9 | void sampleFromPrimitive(DecoratedCloud &cloud, Eigen::MatrixXi inliers_idx, Primitive *plane, int nbSample, double T); 10 | } 11 | #endif 12 | 13 | -------------------------------------------------------------------------------- /RANSAC/include/typedefs.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Aron Monszpart on 19/01/17. 3 | // 4 | 5 | #ifndef ACQ_TYPEDEFS_H 6 | #define ACQ_TYPEDEFS_H 7 | 8 | #include "Eigen/Core" 9 | 10 | // Use this, if using aligned vector types from Eigen 11 | // See also 12 | // http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html 13 | 14 | //#include "Eigen/StdVector" 15 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f) 16 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) 17 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f) 18 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4d) 19 | 20 | #include 21 | #include 22 | 23 | namespace acq { 24 | 25 | //! Dynamically sized matrix of points in rows, a "point cloud". 26 | typedef Eigen::MatrixXd CloudT; 27 | //! Dynamically sized matrix of vectors in rows, a list of normals. 28 | typedef Eigen::MatrixXd NormalsT; 29 | //! Dynamically sized matrix of face vertex indices in rows. 30 | typedef Eigen::MatrixXi FacesT; 31 | // color en float 32 | typedef Eigen::MatrixXd ColorT; 33 | 34 | /** \brief An associative storage of neighbour indices for point cloud 35 | * { pointId => [neighbourId_0, nId_1, ... nId_k-1] } 36 | */ 37 | typedef std::map > NeighboursT; 38 | 39 | } //...ns acq 40 | 41 | #endif //ACQ_TYPEDEFS_H 42 | -------------------------------------------------------------------------------- /RANSAC/project_BUFFIER.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mbuffier/3D_Geometry_Processing/61807fdd0962addcd1907c246e9225876a5292c5/RANSAC/project_BUFFIER.pdf -------------------------------------------------------------------------------- /RANSAC/src/cloudManager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/cloudManager.hpp" 6 | #include 7 | 8 | namespace acq { 9 | 10 | void CloudManager::addCloud(DecoratedCloud const &cloud) { 11 | _clouds.push_back(cloud); 12 | } //...CloudManager::addCloud() 13 | 14 | void CloudManager::setCloud(DecoratedCloud const &cloud, int index) { 15 | if (index >= _clouds.size()) { 16 | if (index != _clouds.size()) 17 | std::cerr << "[CloudManager::setCloud] " 18 | << "Warning, creating " << index - _clouds.size() 19 | << " empty clouds when inserting to index " << index 20 | << ", current size is " << _clouds.size() 21 | << "...why not use addCloud?\n"; 22 | _clouds.resize(index + 1); 23 | } 24 | 25 | _clouds.at(index) = cloud; 26 | } //...CloudManager::setCloud() 27 | 28 | DecoratedCloud &CloudManager::getCloud(int index) { 29 | if (index < _clouds.size()) 30 | return _clouds.at(index); 31 | else { 32 | std::cerr << "Cannot return cloud with id " << index 33 | << ", only have " << _clouds.size() 34 | << " clouds...returning empty cloud\n"; 35 | throw new std::runtime_error("No such cloud"); 36 | } 37 | } //...CloudManager::getCloud() 38 | 39 | DecoratedCloud const &CloudManager::getCloud(int index) const { 40 | return const_cast( 41 | const_cast(this)->getCloud(index) 42 | ); 43 | } //...CloudManager::getCloud() (const) 44 | 45 | 46 | void CloudManager::deleteCloud(int index) { 47 | _clouds.erase(_clouds.begin() + index-1); 48 | } 49 | 50 | void CloudManager::deleteCloudFromIndex(int indexStart) { 51 | std::vector::iterator thisCloudIt; 52 | 53 | for ( thisCloudIt = _clouds.begin()+indexStart; thisCloudIt != _clouds.end(); ) { 54 | thisCloudIt = _clouds.erase(thisCloudIt); 55 | } 56 | } 57 | 58 | void CloudManager::clearCloud() { 59 | std::vector::iterator thisCloudIt; 60 | for ( thisCloudIt = _clouds.begin(); thisCloudIt != _clouds.end(); ) { 61 | thisCloudIt = _clouds.erase(thisCloudIt); 62 | } 63 | } 64 | 65 | } //...ns acq 66 | -------------------------------------------------------------------------------- /RANSAC/src/decoratedCloud.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/decoratedCloud.hpp" 6 | 7 | namespace acq { 8 | 9 | DecoratedCloud::DecoratedCloud(CloudT const& vertices) 10 | : _vertices(vertices) {} 11 | 12 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces) 13 | : _vertices(vertices), _faces(faces) 14 | {} 15 | 16 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals) 17 | : _vertices(vertices), _faces(faces), _normals(normals) 18 | {} 19 | 20 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals, ColorT const& colors) 21 | : _vertices(vertices), _faces(faces), _normals(normals), _colors(colors) 22 | {} 23 | 24 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, NormalsT const& normals) 25 | : _vertices(vertices), _normals(normals) 26 | {} 27 | 28 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, NormalsT const& normals, ColorT const& colors) 29 | : _vertices(vertices), _normals(normals), _colors(colors) 30 | {} 31 | 32 | 33 | } //...ns acq 34 | -------------------------------------------------------------------------------- /RANSAC/src/evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/evaluation.h" 2 | 3 | namespace acq { 4 | 5 | void computeBoundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin, DecoratedCloud& cloud) { 6 | Eigen::MatrixXd maximum(1, 3) ; 7 | maximum = cloud.getVertices().colwise().maxCoeff() ; 8 | Xmax = maximum(0) ; 9 | Ymax= maximum(1) ; 10 | Zmax = maximum(2) ; 11 | 12 | Eigen::MatrixXd minimum(1, 3) ; 13 | minimum = cloud.getVertices().colwise().minCoeff() ; 14 | Xmin = minimum(0) ; 15 | Ymin = minimum(1) ; 16 | Zmin = minimum(2) ; 17 | } 18 | 19 | // add noise to the vertices of the cloud with typeMatrix==1, to the normals with typeMatrix==2 20 | Eigen::MatrixXd addNoise(float noise, DecoratedCloud& cloud, int typeMatrix) { 21 | // compute the value of the boundingBox for the second cloud 22 | float Xmax, Xmin, Ymax, Ymin, Zmax, Zmin ; 23 | computeBoundingBox(Xmax, Xmin, Ymax, Ymin,Zmax, Zmin, cloud) ; 24 | 25 | // set the variance to sigma = noise% of the bouding box size in each direction 26 | float sigmaX =(Xmax-Xmin)*noise ; 27 | float sigmaY = (Ymax-Ymin)*noise ; 28 | float sigmaZ = (Zmax-Zmin)*noise ; 29 | int M ; 30 | 31 | if (typeMatrix==1) 32 | M = cloud.getVertices().rows() ; 33 | else if (typeMatrix==2) 34 | M = cloud.getNormals().rows() ; 35 | 36 | // initialization 37 | Eigen::MatrixXd random(M, 3) ; 38 | // construct noise 39 | for(int i = 0; i< M; i++) { 40 | random(i, 0) = std::rand()*sigmaX /RAND_MAX ; 41 | random(i, 1) = std::rand()*sigmaY /RAND_MAX ; 42 | random(i, 2) = std::rand()*sigmaZ /RAND_MAX ; 43 | } 44 | // add noise to the matrix 45 | Eigen::MatrixXd newMatrix(M,3) ; 46 | if (typeMatrix==1) 47 | newMatrix = cloud.getVertices() + random ; 48 | else if (typeMatrix==2) 49 | newMatrix = cloud.getNormals() + random ; 50 | 51 | return newMatrix ; 52 | } 53 | 54 | void connectedComponentManager(CloudManager& thisCloudManager, PrimitiveManager& best_primitives, double threshold) { 55 | int sizeCloud = thisCloudManager.getCloudSize(), numberDelete = 0, thisType, nbVertices ; 56 | 57 | for (int i = 0; i< sizeCloud; i++) { 58 | // get back the type and the number of vertices for this mesh 59 | thisType = best_primitives.getPrimitive(i-numberDelete)->getType() ; 60 | nbVertices = thisCloudManager.getCloud(i-numberDelete).getVertices().rows() ; 61 | 62 | // apply connected component if it's a plane 63 | if (nbVertices > 8 && thisType==2) { 64 | connectedComponent(thisCloudManager.getCloud(i-numberDelete), threshold) ; 65 | } 66 | 67 | // else if it's a sphere, set the color 68 | else if (nbVertices > 8 && thisType==1) { 69 | Eigen::MatrixXd C(nbVertices,3) ; 70 | 71 | C = Eigen::RowVector3d(std::rand()/double(RAND_MAX), 72 | std::rand()/double(RAND_MAX), 73 | std::rand()/double(RAND_MAX)).replicate(nbVertices, 1); 74 | 75 | thisCloudManager.getCloud(i-numberDelete).setColors(C) ; 76 | } 77 | 78 | // else the cloud is too small and we delete it 79 | else if (nbVertices < 8) { 80 | thisCloudManager.deleteCloud(i-numberDelete) ; 81 | numberDelete += 1 ; 82 | } 83 | } 84 | } 85 | 86 | 87 | void connectedComponent(DecoratedCloud& cloud, double threshold) { 88 | // initialisation 89 | int nbPoints = cloud.getVertices().rows(), dim = cloud.getVertices().cols(), connectivity = 8 ; 90 | Eigen::MatrixXd colors(nbPoints,dim) ; 91 | Eigen::MatrixXd visited(nbPoints,1) ; 92 | visited = Eigen::MatrixXd::Zero(nbPoints, 1) ; 93 | Eigen::MatrixXd vertices(nbPoints,dim) ; 94 | vertices = cloud.getVertices() ; 95 | Eigen::RowVector3d thisLabel = Eigen::RowVector3d(std::rand()/double(RAND_MAX), 96 | std::rand()/double(RAND_MAX), 97 | std::rand()/double(RAND_MAX)) ; 98 | 99 | // construct the kdTree for this cloud 100 | ANNpointArray verticesArray ; 101 | verticesArray = annAllocPts(nbPoints, dim); 102 | verticesArray = matrixToANNArray(cloud.getVertices()) ; 103 | 104 | ANNkd_tree* kdTree = new ANNkd_tree( // build search structure 105 | verticesArray, // the data points 106 | nbPoints, // number of points 107 | dim); // dimension of space 108 | 109 | // for each vertices, test if test is satisfied 110 | for (int i = 0 ; i < nbPoints; i++) { 111 | if (visited(i,0) == 0) { 112 | // label with the current color if unvisited yet 113 | labelVertices(thisLabel, verticesArray, colors, i, visited, kdTree, threshold, connectivity) ; 114 | 115 | // change the label 116 | thisLabel = Eigen::RowVector3d(std::rand()/double(RAND_MAX), 117 | std::rand()/double(RAND_MAX), 118 | std::rand()/double(RAND_MAX)) ; 119 | } 120 | } 121 | // store the result 122 | cloud.setColors(colors) ; 123 | 124 | // free the memory used 125 | delete kdTree ; 126 | annDeallocPts(verticesArray) ; 127 | } 128 | 129 | void labelVertices(Eigen::RowVector3d thisColor, ANNpointArray verticesArray, Eigen::MatrixXd& colors, 130 | int this_idx, Eigen::MatrixXd& visited, ANNkd_tree* kdTree, double threshold, int connectivity) { 131 | // store the result for this vertices, visited and the color 132 | visited(this_idx, 0) = 1 ; 133 | colors.row(this_idx) = thisColor ; 134 | 135 | // chose to selectione the closest "connectivity" points 136 | // allocate variable for the kdTree 137 | ANNidxArray nnIdx; 138 | nnIdx = new ANNidx[connectivity]; // the index 139 | ANNdistArray dists; 140 | dists = new ANNdist[connectivity]; //the distance 141 | ANNpoint queryPt; 142 | queryPt = annAllocPt(3); 143 | queryPt = verticesArray[this_idx] ; // the point 144 | 145 | // search in the kdTree 146 | kdTree->annkSearch( // search 147 | queryPt, // query point 148 | connectivity, // number of near neighbors 149 | nnIdx, // nearest neighbors (returned) 150 | dists, // distance (returned) 151 | 0.0); 152 | 153 | // to store the results 154 | int indexFind ; 155 | double distance ; 156 | 157 | for (int thisNeigh = 0; thisNeigh< connectivity; thisNeigh++) { 158 | // take the result 159 | indexFind = nnIdx[thisNeigh] ; 160 | distance = dists[thisNeigh] ; 161 | 162 | // if it hasn't been visited yet 163 | if (visited(indexFind, 0) == 0) { 164 | // and the distance is bellow threshold : label recursively 165 | if (distance < threshold) { 166 | labelVertices(thisColor, verticesArray, colors, indexFind,visited, kdTree, threshold, connectivity) ; 167 | } 168 | } 169 | } 170 | // free memory 171 | delete [] nnIdx ; 172 | delete [] dists; 173 | annDeallocPt(queryPt) ; 174 | } 175 | 176 | // function to transform a matrix into a ANNpointArray 177 | ANNpointArray matrixToANNArray(Eigen::MatrixXd const& points) { 178 | unsigned int M = points.rows() ; 179 | int dim = 3 ; 180 | ANNpointArray dataPts; 181 | dataPts = annAllocPts(M, dim); // allocate data points 182 | 183 | // create and allocate memory for each point 184 | for (int i=0; i sampleInd(3,1) ; 11 | int newIndex ; 12 | // add a random indices between 0 and sizeMatrix in a numberPoint sized vector 13 | for (int i=0; i<3; i++) { 14 | bool isUnique = false; 15 | while(!isUnique){ // we shouldn't use twice the same point 16 | newIndex = rand() % (cloudSize + 1) ; 17 | isUnique = true; 18 | for(int j=0; j thisCenter = computerCenter(vertices.topRows(2), normals.topRows(2)) ; 45 | double estimatedRadius = computerRadius(vertices.topRows(2), thisCenter) ; 46 | 47 | // compute the estimated normal for the last point 48 | Eigen::Matrix estimatedNormal = vertices.row(2) - thisCenter ; 49 | estimatedNormal.normalize() ; 50 | 51 | // test for the radius 52 | double test1 = computerRadius(vertices.row(2), thisCenter) - estimatedRadius ; 53 | double test2 = estimatedNormal.dot(normals.row(2).normalized()) ; 54 | double test3 = estimatedNormal.dot(-normals.row(2).normalized()) ; // in case where all the normals are inverted 55 | 56 | int thisReturn = 0 ; 57 | 58 | if (std::abs(test1) < threshold ) { 59 | if ( test2 > alpha || test3 > alpha) { 60 | // if the 2 test are true, the 3 points form a sphere 61 | thisReturn = 1 ; 62 | } 63 | } 64 | return thisReturn ; 65 | } 66 | 67 | // if the 3 points create a sphere, we add it to the primitives 68 | void computeSphere(Eigen::Matrix sample_idx, Eigen::Matrix3d variance, DecoratedCloud& cloud, PrimitiveManager& primitives, double threshold, double alpha) { 69 | Eigen::MatrixXd vertices = cloud.getVertices() ; 70 | Eigen::MatrixXd normals = cloud.getNormals() ; 71 | int cloudSize = vertices.rows() ; 72 | 73 | Eigen::Matrix3d thisSampledVertices ; 74 | Eigen::Matrix3d thisSampledNormals ; 75 | 76 | // extract the 3 points sampled by the indices 77 | for(int i =0 ; i< 3; i++) { 78 | thisSampledVertices.row(i) = vertices.row(sample_idx(i,0)) ; 79 | thisSampledNormals.row(i) = normals.row(sample_idx(i,0)) ; 80 | } 81 | // test if it's a sphere 82 | if (isSphere(thisSampledVertices, thisSampledNormals, threshold, alpha)) { 83 | // compute the attribut for the object 84 | Eigen::Matrix thisCenter = computerCenter(thisSampledVertices, thisSampledNormals) ; 85 | double thisRadius = computerRadius(thisSampledVertices, thisCenter) ; 86 | 87 | // create the object and compute its score 88 | Primitive* thisSphere = new Sphere(thisRadius, thisCenter) ; 89 | thisSphere->computeScore(variance, cloud, threshold, alpha) ; 90 | 91 | // set the type 92 | thisSphere->setType(1) ; 93 | 94 | // store it in the cloud primitive 95 | primitives.addPrimitive(thisSphere) ; 96 | } 97 | } 98 | 99 | // compute the center of a shpere by finding the better intersection possible using least square computation 100 | Eigen::Matrix computerCenter(Eigen::MatrixXd vertices, Eigen::MatrixXd normals) { 101 | Eigen::Matrix3d R = Eigen::Matrix3d::Zero(3,3) ; 102 | Eigen::Matrix q = Eigen::MatrixXd::Zero(3,1) ; 103 | Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3,3) ; 104 | int numberPoint = vertices.rows() ; 105 | Eigen::Matrix thisNormal,thisPosition ; 106 | 107 | // fill the system 108 | for (int i = 0; i< numberPoint; i++) { 109 | thisNormal = normals.row(i) ; 110 | thisPosition = vertices.row(i) ; 111 | 112 | R += I - thisNormal.transpose()*thisNormal ; 113 | 114 | q += (I - thisNormal.transpose()*thisNormal) * thisPosition.transpose() ; 115 | } 116 | 117 | // solve the system using least jacobi decomposition 118 | Eigen::Matrix thisCenter ; 119 | thisCenter = R.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(q) ; 120 | 121 | return thisCenter.transpose() ; 122 | } 123 | 124 | double computerRadius(Eigen::MatrixXd thisVertices, Eigen::Matrix thisCenter) { 125 | // compute the distance between each point and the center 126 | int numberPoint = thisVertices.rows() ; 127 | Eigen::MatrixXd distances(numberPoint,1) ; 128 | distances = (thisVertices-thisCenter.replicate(numberPoint,1)).rowwise().norm() ; 129 | 130 | // compute the mean and return it 131 | double meanRadius = distances.mean() ; 132 | return meanRadius ; 133 | } 134 | 135 | /********* ============= Functions to handle PLANE =============== *********/ 136 | void computePlane(Eigen::Matrix sample_idx, 137 | Eigen::Matrix3d variance, 138 | DecoratedCloud &cloud, 139 | PrimitiveManager &primitives, 140 | double thresh, double alpha) { 141 | Eigen::MatrixXd V = cloud.getVertices(), N = cloud.getNormals(); 142 | 143 | const int cloudSize = V.rows(); 144 | const int nSamples = sample_idx.rows(); 145 | 146 | 147 | // ---- Retrieve the N vertices and their normals ---- 148 | Eigen::Matrix3d thisVertex, thisNormal; 149 | for (int i = 0; i < 3; i++) { 150 | thisVertex.row(i) = V.row(sample_idx(i, 0)); 151 | thisNormal.row(i) = N.row(sample_idx(i, 0)); 152 | } 153 | 154 | if (isPlane(thisVertex, thisNormal, thresh, alpha)) { 155 | // ---- Create a new plane and compute its score ---- 156 | Eigen::Matrix planeNormal = computeNormal(thisVertex, thisNormal.row(0)); 157 | Eigen::Matrix planeRefPoint = thisVertex.colwise().mean(); 158 | 159 | Primitive* newPlane = new Plane(planeRefPoint, planeNormal); 160 | newPlane->computeScore(variance, cloud, thresh, alpha); 161 | newPlane->setType(2) ; 162 | 163 | // ---- Store it in the cloudPrimitive ---- 164 | primitives.addPrimitive(newPlane); 165 | } 166 | } 167 | 168 | /*** ----------- isPlane() ----------- ***/ 169 | bool isPlane(Eigen::MatrixXd V, Eigen::MatrixXd N, double T, double alpha) { 170 | Eigen::Matrix planeNormal = computeNormal(V, N.row(0)); 171 | bool isPlane = true; 172 | for (int i = 0; i < N.rows(); i++) { 173 | if (std::abs(N.row(i).dot(planeNormal)) < alpha) isPlane = false; 174 | } 175 | return isPlane; 176 | } 177 | 178 | /*** ----------- computeNormal() ----------- ***/ 179 | Eigen::Matrix computeNormal(Eigen::MatrixXd V, Eigen::Matrix _N) { 180 | Eigen::Matrix N; N << 0.0,0.0,0.0; 181 | Eigen::Matrix P01, P02 ; 182 | 183 | for (int i = 0; i < V.rows() - 2; i++) { 184 | P01 = V.row(1 + i) - V.row(i); 185 | P02 = V.row(2 + i) - V.row(i); 186 | N += P02.cross(P01) / (V.rows() - 2); 187 | } 188 | 189 | // Check for normal orientation 190 | //if (_N.dot(N) < 0) N = -N; 191 | // Normalize 192 | if(N(0,0)!=0 && N(0,1)!=0 && N(0,2)!=0) N = N.normalized(); 193 | return N; 194 | } 195 | 196 | 197 | 198 | /********* ============= Functions to handle the final cloud =============== *********/ 199 | // fuse the cloud with the same primitives 200 | void fuse(PrimitiveManager& best_primitives, 201 | CloudManager& clouds, 202 | double T_rad, // Radius Distance Threshold (Sphere) 203 | double T_cent, // Center Distance Threshold (Sphere) 204 | double T_norm, // Normals Distance Threshold (Plane) 205 | double T_refPt // RefPoint Distance Threshold (Plane) 206 | ) { 207 | 208 | // initialization 209 | int nbCloudInitial = clouds.getCloudSize(); 210 | Eigen::MatrixXi visited = Eigen::MatrixXi::Zero(1, nbCloudInitial) ; 211 | double d1,d2, d3 ; 212 | int current_label = 0 ; 213 | // === Consider every pair of primitive for merging === 214 | Primitive* first_prim, *second_prim; 215 | 216 | for(int i=0; i < nbCloudInitial; i++){ // First Primitive 217 | if (visited(0,i) == 0) { 218 | current_label = current_label+1 ; 219 | visited(0,i) = current_label ; 220 | 221 | first_prim = best_primitives.getPrimitive(i); 222 | 223 | for(int j=i+1; jgetType()==second_prim->getType()){ 228 | 229 | // ---- They are both Spheres --- 230 | if(first_prim->getType()==1){ 231 | d1 = (static_cast(first_prim)->getCenter() - static_cast(second_prim)->getCenter()).norm(); 232 | d2 = std::abs(static_cast(first_prim)->getRadius() - static_cast(second_prim)->getRadius()); 233 | 234 | if(d1 < T_cent && d2 < T_rad){ 235 | visited(0,j) = current_label ; 236 | } 237 | }// ---- They are both Planes --- 238 | else{ 239 | d1 = std::abs((static_cast(first_prim)->getNormal().dot(static_cast(second_prim)->getNormal()))); 240 | d2 = std::abs((static_cast(first_prim)->getRefPoint() - static_cast(second_prim)->getRefPoint()).dot(static_cast(second_prim)->getNormal())); 241 | d3 = std::abs((static_cast(first_prim)->getRefPoint() - static_cast(second_prim)->getRefPoint()).dot(static_cast(first_prim)->getNormal())); 242 | 243 | if(d1 > T_norm && (d2 < T_refPt || d3 < T_refPt)){ 244 | visited(0,j) = current_label ; 245 | } 246 | } 247 | } 248 | } 249 | } 250 | } 251 | // need to fuse the meshes 252 | int numberOfVertices = 0 ; 253 | int thisType ; 254 | 255 | // === Merge all the primitives in the cloudManager === 256 | for(int thisLabel=1; thisLabel<=current_label; thisLabel++) { 257 | 258 | // determine the size of the new vertices for this cloud 259 | for(int j=0; j< nbCloudInitial; j++) { 260 | if (visited(0,j) == thisLabel) { 261 | numberOfVertices += clouds.getCloud(j).getVertices().rows() ; 262 | } 263 | } 264 | 265 | // initialisation to fuse everything 266 | // create the new matrices 267 | Eigen::MatrixXd V(numberOfVertices, 3) ; 268 | Eigen::MatrixXd N(numberOfVertices, 3) ; 269 | Eigen::MatrixXd C(numberOfVertices,3) ; 270 | 271 | int indiceStart = 0 ; 272 | int nbVertCloud = 0; 273 | 274 | for(int j=0; j< nbCloudInitial; j++){ 275 | if (visited(0,j) == thisLabel) { 276 | // ****** FUSE THE CLOUDS ****** 277 | // number of vertex for this cloud 278 | nbVertCloud = clouds.getCloud(j).getVertices().rows() ; 279 | 280 | // fill each block 281 | V.block(indiceStart,0,nbVertCloud,3) = clouds.getCloud(j).getVertices(); 282 | 283 | N.block(indiceStart,0,nbVertCloud,3) = clouds.getCloud(j).getNormals(); 284 | 285 | C.block(indiceStart,0,nbVertCloud,3) = Eigen::RowVector3d(std::rand()/double(RAND_MAX), 286 | std::rand()/double(RAND_MAX), 287 | std::rand()/double(RAND_MAX)).replicate( 288 | clouds.getCloud(j).getVertices().rows(), 1); 289 | 290 | // update the indice to start filling 291 | indiceStart += nbVertCloud ; 292 | 293 | // ****** FUSE THE PRIMITIVES (only the type) ****** 294 | if (best_primitives.getPrimitive(j)->getType() == 1) { 295 | thisType = 1 ; 296 | } 297 | if (best_primitives.getPrimitive(j)->getType() == 2) { 298 | thisType = 2 ; 299 | } 300 | } 301 | } 302 | // set the new cloud 303 | clouds.setCloud(DecoratedCloud(V,N,C), thisLabel-1) ; 304 | numberOfVertices = 0; 305 | 306 | // set the new primitive type 307 | Primitive* newPrimitive = new Primitive() ; 308 | if (thisType == 1) { 309 | newPrimitive->setType(1) ; 310 | } 311 | else if (thisType == 2) { 312 | newPrimitive->setType(2) ; 313 | } 314 | 315 | best_primitives.setPrimitive(newPrimitive, thisLabel-1) ; 316 | } 317 | 318 | // clean the rest of the managers to avoid memory leak 319 | if(clouds.getCloudSize() - current_label > 0) { 320 | clouds.deleteCloudFromIndex(current_label+1); 321 | best_primitives.deleteCloudFromIndex(current_label+1) ; 322 | } 323 | } 324 | 325 | 326 | // take a cloudManager and gather all the cloud in one 327 | DecoratedCloud* gatherClouds(CloudManager& cloudManager, int colorExit) { 328 | // === Build new cloud with color attributes === 329 | int numberOfCloud = cloudManager.getCloudSize() ; 330 | int numberOfVertices =0 ; 331 | 332 | // determine the size of the new cloud 333 | for(int i=0; i< numberOfCloud; i++){ 334 | numberOfVertices += cloudManager.getCloud(i).getVertices().rows() ; 335 | } 336 | 337 | // create the matrix to store the result 338 | Eigen::MatrixXd V(numberOfVertices,3) ; 339 | Eigen::MatrixXd C(numberOfVertices,3) ; 340 | Eigen::MatrixXd N(numberOfVertices,3); 341 | 342 | int indiceStart = 0 ; 343 | int nbVertCloud ; 344 | 345 | for(int i=0; i< numberOfCloud; i++){ 346 | // number of vertex for this cloud 347 | nbVertCloud = cloudManager.getCloud(i).getVertices().rows() ; 348 | 349 | // fill each block 350 | V.block(indiceStart,0,nbVertCloud,3) = cloudManager.getCloud(i).getVertices(); 351 | 352 | N.block(indiceStart,0,nbVertCloud,3) = cloudManager.getCloud(i).getNormals(); 353 | 354 | if (colorExit==1) { 355 | C.block(indiceStart,0,nbVertCloud,3) = cloudManager.getCloud(i).getColors(); 356 | } 357 | else { 358 | C.block(indiceStart,0,nbVertCloud,3) = Eigen::RowVector3d(std::rand()/double(RAND_MAX), 359 | std::rand()/double(RAND_MAX), 360 | std::rand()/double(RAND_MAX)).replicate( 361 | cloudManager.getCloud(i).getVertices().rows(), 1); 362 | } 363 | 364 | // update the indice to start filling 365 | indiceStart += nbVertCloud ; 366 | } 367 | 368 | // ---- Create new Cloud --- 369 | DecoratedCloud* newCloud = new DecoratedCloud(V,N,C) ; 370 | return newCloud ; 371 | } 372 | 373 | // remove the inliers from the main cloud and store them in the cloudManager 374 | void cleanCloud(DecoratedCloud& cloudRansac, CloudManager& cloudManager, Eigen::MatrixXi inliers_idx){ 375 | // ---- We remove inliers from cloudRansac ----- 376 | int n_inliers = inliers_idx.rows(); 377 | 378 | if(n_inliers > 0) { 379 | int n_cloud = cloudRansac.getVertices().rows(); 380 | Eigen::MatrixXd V_in(n_cloud, 3); 381 | Eigen::MatrixXd V_out(n_cloud, 3) ; 382 | Eigen::MatrixXd N_in(n_cloud, 3) ; 383 | Eigen::MatrixXd N_out(n_cloud, 3); 384 | 385 | int inliers_valid = 0, outliers_valid = 0; 386 | 387 | // ---- For every vertex, search if is an inlier --- 388 | for(int i=0; i 13 | 14 | namespace acq { 15 | 16 | /** \brief Re-estimate normals of cloud \p V fitting planes 17 | * to the \p kNeighbours nearest neighbours of each point. 18 | * \param[in ] kNeighbours How many neighbours to use (Typiclaly: 5..15) 19 | * \param[in ] vertices Input pointcloud. Nx3, where N is the number of points. 20 | * \param[in ] maxNeighbourDist Maximum distance between vertex and neighbour. 21 | * \param[out] viewer The viewer to show the normals at. 22 | * \return The estimated normals, Nx3. 23 | */ 24 | NormalsT 25 | recalcNormals( 26 | int const kNeighbours, 27 | CloudT const& vertices, 28 | float const maxNeighbourDist 29 | ) { 30 | NeighboursT const neighbours = 31 | calculateCloudNeighbours( 32 | /* [in] cloud: */ vertices, 33 | /* [in] k-neighbours: */ kNeighbours, 34 | /* [in] maxDist: */ maxNeighbourDist 35 | ); 36 | 37 | // Estimate normals for points in cloud vertices 38 | NormalsT normals = 39 | calculateCloudNormals( 40 | /* [in] Cloud: */ vertices, 41 | /* [in] Lists of neighbours: */ neighbours 42 | ); 43 | 44 | return normals; 45 | } //...recalcNormals() 46 | 47 | void setViewerNormals( 48 | igl::viewer::Viewer & viewer, 49 | CloudT const& vertices, 50 | NormalsT const& normals 51 | ) { 52 | // [Optional] Set viewer face normals for shading 53 | //viewer.data.set_normals(normals); 54 | 55 | // Clear visualized lines (see Viewer.clear()) 56 | viewer.data.lines = Eigen::MatrixXd(0, 9); 57 | 58 | // Add normals to viewer 59 | viewer.data.add_edges( 60 | /* [in] Edge starting points: */ vertices, 61 | /* [in] Edge endpoints: */ vertices + normals * 0.01, // scale normals to 1% length 62 | /* [in] Colors: */ Eigen::Vector3d::Zero() 63 | ); 64 | } 65 | 66 | } //...ns acq 67 | 68 | int main(int argc, char *argv[]) { 69 | 70 | // How many neighbours to use for normal estimation, shown on GUI. 71 | int kNeighbours = 10; 72 | // Maximum distance between vertices to be considered neighbours (FLANN mode) 73 | float maxNeighbourDist = 0.15; //TODO: set to average vertex distance upon read 74 | 75 | // ********* VARIABLES FOR THE ALGORITHM ********* 76 | int nbIteration = 10000 ; 77 | int samplePerIt = 50 ; 78 | double thresh = 0.001 ; 79 | double alpha = 0.999 ; 80 | double thresh_best = 80.0 ; 81 | float noise = 0.6 ; 82 | int numberOfOldMesh = 3 ; 83 | double thresCC = 0.001 ; 84 | 85 | double T_rad = 0.01 ; 86 | double T_cent = 0.01 ; 87 | double T_norm = 0.98 ; 88 | double T_refPt = 0.01 ; 89 | 90 | // will store the current primitives and the point cloud per primitives 91 | acq::PrimitiveManager best_primitives ; 92 | acq::CloudManager cloudManagerParts ; 93 | 94 | // deals with several meshes 95 | enum MeshType { mesh1=0, mesh2, mesh3} typeMesh = mesh1 ; 96 | //************************************ 97 | 98 | // Load a mesh in OFF format 99 | std::string meshPath1 = "../models/scene_3.off"; 100 | if (argc > 1) { 101 | meshPath1 = std::string(argv[1]); 102 | if (meshPath1.find(".obj") == std::string::npos) { 103 | std::cerr << "Only ready for OBJ files for now...\n"; 104 | return EXIT_FAILURE; 105 | } 106 | } else { 107 | std::cout << "Usage: iglFrameWork ." << "\n"; 108 | } 109 | 110 | std::string meshPath2 = "../models/scene_2.off"; 111 | if (argc > 1) { 112 | meshPath2 = std::string(argv[1]); 113 | if (meshPath2.find(".obj") == std::string::npos) { 114 | std::cerr << "Only ready for OBJ files for now...\n"; 115 | return EXIT_FAILURE; 116 | } 117 | } else { 118 | std::cout << "Usage: iglFrameWork ." << "\n"; 119 | } 120 | 121 | std::string meshPath3 = "../models/cube_damaged.off"; 122 | if (argc > 1) { 123 | meshPath2 = std::string(argv[1]); 124 | if (meshPath2.find(".obj") == std::string::npos) { 125 | std::cerr << "Only ready for OBJ files for now...\n"; 126 | return EXIT_FAILURE; 127 | } 128 | } else { 129 | std::cout << "Usage: iglFrameWork ." << "\n"; 130 | } 131 | 132 | // Visualize the mesh in a viewer 133 | igl::viewer::Viewer viewer; 134 | viewer.core.show_lines = false; 135 | viewer.core.show_overlay = false; 136 | 137 | // Store cloud so we can store normals later 138 | acq::CloudManager cloudManagerOldMesh; 139 | // Read mesh from meshPath 140 | { 141 | // == ******** For the first mesh ******* == 142 | Eigen::MatrixXd V; 143 | Eigen::MatrixXi F; 144 | igl::readOFF(meshPath1, V, F); 145 | 146 | if (V.rows() <= 0) { 147 | std::cerr << "Could not read mesh at " << meshPath1 148 | << "...exiting...\n"; 149 | return EXIT_FAILURE; 150 | } 151 | 152 | // ----- Normalize Vertices ----- 153 | Eigen::MatrixXd max_row = V.rowwise().maxCoeff(); 154 | Eigen::MatrixXd max_col = V.colwise().maxCoeff(); 155 | V /= std::max(max_row.maxCoeff(), max_col.maxCoeff()); 156 | 157 | // == ******** For the second mesh ******* == 158 | 159 | Eigen::MatrixXd V2; 160 | Eigen::MatrixXi F2; 161 | igl::readOFF(meshPath2, V2, F2); 162 | 163 | if (V2.rows() <= 0) { 164 | std::cerr << "Could not read mesh at " << meshPath2 165 | << "...exiting...\n"; 166 | return EXIT_FAILURE; 167 | } 168 | 169 | // ----- Normalize Vertices ----- 170 | Eigen::MatrixXd max_row2 = V2.rowwise().maxCoeff(); 171 | Eigen::MatrixXd max_col2 = V2.colwise().maxCoeff(); 172 | V2 /= std::max(max_row2.maxCoeff(), max_col2.maxCoeff()); 173 | 174 | // == ******** For the third mesh ******* == 175 | 176 | Eigen::MatrixXd V3; 177 | Eigen::MatrixXi F3; 178 | igl::readOFF(meshPath3, V3, F3); 179 | 180 | if (V3.rows() <= 0) { 181 | std::cerr << "Could not read mesh at " << meshPath3 182 | << "...exiting...\n"; 183 | return EXIT_FAILURE; 184 | } 185 | 186 | // ----- Normalize Vertices ----- 187 | Eigen::MatrixXd max_row3 = V3.rowwise().maxCoeff(); 188 | Eigen::MatrixXd max_col3 = V3.colwise().maxCoeff(); 189 | V3 /= std::max(max_row3.maxCoeff(), max_col3.maxCoeff()); 190 | 191 | // 2 times to be able to reload it easily 192 | for (int i=0; i<2; i++) { 193 | cloudManagerOldMesh.addCloud(acq::DecoratedCloud(V, F)); 194 | cloudManagerOldMesh.addCloud(acq::DecoratedCloud(V2, F2)); 195 | cloudManagerOldMesh.addCloud(acq::DecoratedCloud(V3, F3)); 196 | } 197 | 198 | // set the mesh 199 | viewer.data.clear() ; 200 | 201 | // Show mesh 202 | viewer.data.set_mesh( 203 | cloudManagerOldMesh.getCloud(typeMesh).getVertices(), 204 | cloudManagerOldMesh.getCloud(typeMesh).getFaces() 205 | ); 206 | 207 | } 208 | 209 | // Extend viewer menu using a lambda function 210 | viewer.callback_init = 211 | [ 212 | &cloudManagerOldMesh, &kNeighbours, &maxNeighbourDist, 213 | &nbIteration, &samplePerIt, 214 | &best_primitives, &cloudManagerParts, &thresh, &alpha, &thresh_best, 215 | &typeMesh, &noise, &numberOfOldMesh, &thresCC, &T_rad, &T_cent, &T_norm, &T_refPt 216 | ] (igl::viewer::Viewer& viewer) 217 | { 218 | // Add an additional menu window 219 | viewer.ngui->addWindow(Eigen::Vector2i(900,10), "Acquisition3D"); 220 | 221 | viewer.ngui->addGroup("Choose your mesh"); 222 | 223 | viewer.ngui->addVariable("Which mesh do you want ?",typeMesh)->setItems( 224 | {"Sphere & Cube","Scene", "Planes"} 225 | ); 226 | 227 | viewer.ngui->addButton("Show the original mesh", 228 | [&]() { 229 | viewer.data.clear() ; 230 | // Show mesh 231 | viewer.data.set_mesh( 232 | cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getVertices(), 233 | cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getFaces() 234 | ); 235 | 236 | // clean all the primitives 237 | best_primitives.clearAllPrimitives() ; 238 | 239 | // clear the cloudManager 240 | cloudManagerParts.clearCloud() ; 241 | 242 | // replace by the original mesh 243 | cloudManagerOldMesh.setCloud(cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh), typeMesh) ; 244 | }); 245 | 246 | viewer.ngui->addButton("Compute Normals", 247 | [&]() { 248 | 249 | cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).setNormals( 250 | acq::recalcNormals( 251 | /* [in] K-neighbours for FLANN: */ kNeighbours, 252 | /* [in] Vertices matrix: */ cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getVertices(), 253 | /* [in] max neighbour distance: */ maxNeighbourDist 254 | ) 255 | ); 256 | 257 | // Estimate neighbours using FLANN 258 | acq::NeighboursT const neighbours = 259 | acq::calculateCloudNeighboursFromFaces( 260 | /* [in] Faces: */ cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getFaces() 261 | ); 262 | 263 | // Estimate normals for points in cloud vertices 264 | cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).setNormals( 265 | acq::calculateCloudNormals( 266 | /* [in] Cloud: */ cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getVertices(), 267 | /* [in] Lists of neighbours: */ neighbours 268 | ) 269 | ); 270 | 271 | int nFlips = 272 | acq::orientCloudNormalsFromFaces( 273 | /* [in ] Lists of neighbours: */ cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getFaces(), 274 | /* [in,out] Normals to change: */ cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getNormals() 275 | ); 276 | 277 | // also set to the second mesh 278 | 279 | // Estimate normals for points in cloud vertices 280 | cloudManagerOldMesh.getCloud(typeMesh).setNormals( 281 | cloudManagerOldMesh.getCloud(typeMesh+numberOfOldMesh).getNormals() 282 | ); 283 | 284 | 285 | viewer.data.clear() ; 286 | 287 | // Show mesh 288 | viewer.data.set_mesh( 289 | cloudManagerOldMesh.getCloud(typeMesh).getVertices(), 290 | cloudManagerOldMesh.getCloud(typeMesh).getFaces() 291 | ); 292 | 293 | // Update viewer 294 | acq::setViewerNormals( 295 | /* [in, out] Viewer to update: */ viewer, 296 | /* [in] Pointcloud: */ cloudManagerOldMesh.getCloud(typeMesh).getVertices(), 297 | /* [in] Normals of Pointcloud: */ cloudManagerOldMesh.getCloud(typeMesh).getNormals() 298 | ); 299 | 300 | }); 301 | 302 | 303 | viewer.ngui->addGroup("Choose the parameters"); 304 | 305 | // ask for the number of global iteration 306 | viewer.ngui->addVariable("Number of iteration : ",[&] (int val) { 307 | nbIteration = val; }, 308 | [&]() { 309 | return nbIteration; 310 | } ); 311 | 312 | viewer.ngui->addVariable("Sample per iteration : ",[&] (int val) { 313 | samplePerIt = val; }, 314 | [&]() { 315 | return samplePerIt; 316 | } ); 317 | 318 | viewer.ngui->addVariable("Threshold for distance : ",[&] (double val) { 319 | thresh = val; }, 320 | [&]() { 321 | return thresh; 322 | } ); 323 | 324 | viewer.ngui->addVariable("Threshold for angles : ",[&] (double val) { 325 | alpha = val; }, 326 | [&]() { 327 | return alpha; 328 | } ); 329 | 330 | viewer.ngui->addVariable("Threshold for best primitive (%): ",[&] (double val) { 331 | thresh_best = val; }, 332 | [&]() { 333 | return thresh_best; 334 | } ); 335 | 336 | viewer.ngui->addButton("RANSAC", 337 | [&]() { 338 | // get back the cloud we want to work on 339 | acq::DecoratedCloud thisCloud = cloudManagerOldMesh.getCloud(typeMesh) ; 340 | int nbVertices = thisCloud.getVertices().rows() ; 341 | //std::cout << "number of original vertices : " << nbVertices << std::endl ; 342 | 343 | // apply RANSAC 344 | bool ransacSuccess = ransac(thisCloud, best_primitives, cloudManagerParts, 345 | thresh, alpha, thresh_best, nbIteration, samplePerIt) ; 346 | 347 | if (ransacSuccess) { 348 | // fuse the result in the new cloud 349 | acq::DecoratedCloud* newCloud = gatherClouds(cloudManagerParts,0) ; 350 | 351 | // for evaluation 352 | /*std::cout << "number of new vertices : " << newCloud->getVertices().rows() << std::endl ; 353 | float percentage = (float(newCloud->getVertices().rows())/float(nbVertices))*100.f ; 354 | std::cout << "percentage of detection : " << percentage << std::endl ;*/ 355 | 356 | viewer.data.clear() ; 357 | 358 | // Show mesh 359 | viewer.data.set_points(newCloud->getVertices(), newCloud->getColors()) ; 360 | viewer.core.show_overlay = true; 361 | } 362 | 363 | else { 364 | std::cout << "RANSAC didn't find any primitive" << std::endl ; 365 | } 366 | }); 367 | 368 | viewer.ngui->addVariable("Threshold for radius sphere (%): ",[&] (double val) { 369 | T_rad = val; }, 370 | [&]() { 371 | return T_rad; 372 | } ); 373 | 374 | viewer.ngui->addVariable("Threshold for center sphere (%): ",[&] (double val) { 375 | T_cent = val; }, 376 | [&]() { 377 | return T_cent; 378 | } ); 379 | 380 | viewer.ngui->addVariable("Threshold normal plane (%): ",[&] (double val) { 381 | T_norm = val; }, 382 | [&]() { 383 | return T_norm; 384 | } ); 385 | 386 | 387 | viewer.ngui->addVariable("Threshold for reference point (%): ",[&] (double val) { 388 | T_refPt = val; }, 389 | [&]() { 390 | return T_refPt; 391 | } ); 392 | 393 | viewer.ngui->addButton("Primitive fusion", 394 | [&]() { 395 | // fuse the similar primitive in cloud manager 396 | fuse(best_primitives, cloudManagerParts, T_rad, T_cent, T_norm, T_refPt) ; 397 | 398 | // fuse the result in the new cloud with random color 399 | acq::DecoratedCloud* newCloud = gatherClouds(cloudManagerParts,0) ; 400 | 401 | // visualisation 402 | viewer.data.clear() ; 403 | 404 | // Show mesh 405 | viewer.data.set_points(newCloud->getVertices(), newCloud->getColors()) ; 406 | viewer.core.show_overlay = true; 407 | 408 | }); 409 | 410 | viewer.ngui->addVariable("Threshold connective comp :",[&] (double val) { 411 | thresCC = val; }, 412 | [&]() { 413 | return thresCC; 414 | } ); 415 | 416 | viewer.ngui->addButton("Connected components", 417 | [&]() { 418 | connectedComponentManager(cloudManagerParts, best_primitives, thresCC) ; 419 | 420 | // fuse the result in the new cloud with the previous color computed in connected comp 421 | acq::DecoratedCloud* newCloud = gatherClouds(cloudManagerParts,1); 422 | 423 | viewer.data.clear() ; 424 | 425 | // Show mesh 426 | viewer.data.set_points(newCloud->getVertices(), newCloud->getColors()) ; 427 | viewer.core.show_overlay = true; 428 | }); 429 | 430 | /// ----- RECONSTRUCTION ---- 431 | viewer.ngui->addButton("Reconstruction", [&]() { 432 | 433 | int nbSample = 1000; 434 | double T = 0.1; 435 | 436 | acq::DecoratedCloud* newCloud = gatherClouds(cloudManagerParts, 0) ; 437 | acq::DecoratedCloud cloud = acq::DecoratedCloud(newCloud->getVertices(),newCloud->getNormals(),newCloud->getColors()); 438 | 439 | reconstruct(best_primitives, cloud, nbSample, thresh, alpha, T); 440 | 441 | // Show mesh 442 | viewer.data.clear() ; 443 | viewer.data.set_points(cloud.getVertices(), cloud.getColors()) ; 444 | viewer.core.show_overlay = true; 445 | }); 446 | 447 | viewer.ngui->addGroup("Test noise"); 448 | 449 | viewer.ngui->addVariable( "% of noise", [&] (float val) { 450 | noise = val;}, [&]() { 451 | return noise; // get 452 | } ); 453 | 454 | // adding noise for evaluation 455 | viewer.ngui->addButton("Add noise",[&]() { 456 | // noise the position of the vertex 457 | cloudManagerOldMesh.getCloud(typeMesh).setVertices(addNoise(noise,cloudManagerOldMesh.getCloud(typeMesh),1)) ; 458 | 459 | // noise the normals 460 | cloudManagerOldMesh.getCloud(typeMesh).setNormals(addNoise(noise,cloudManagerOldMesh.getCloud(typeMesh),2)); 461 | 462 | viewer.data.clear() ; 463 | 464 | viewer.data.set_mesh( 465 | cloudManagerOldMesh.getCloud(typeMesh).getVertices(), 466 | cloudManagerOldMesh.getCloud(typeMesh).getFaces()) ; 467 | }) ; 468 | 469 | // Generate menu 470 | viewer.screen->performLayout(); 471 | 472 | return false; 473 | }; //...viewer menu 474 | 475 | 476 | // Start viewer 477 | viewer.launch(); 478 | 479 | return 0; 480 | } //...main() 481 | -------------------------------------------------------------------------------- /RANSAC/src/plane.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/impl/primitive.hpp" 2 | 3 | namespace acq { 4 | 5 | /// ------- computeScore() ------ 6 | void Plane::computeScore(Eigen::Matrix3d var, DecoratedCloud& cloud, double T, double alpha) { 7 | 8 | // --- Compute the Plane Inliers --- 9 | Eigen::MatrixXi inliers_idx = this->computeInliers(cloud,T,alpha) ; 10 | if(inliers_idx.rows() > 0) { 11 | this->setInliers_idx(inliers_idx); 12 | 13 | 14 | // --- Estimate the density of our plane --- 15 | //double inliersDensity = inliers_idx.rows()/this->findInliersBoundingBox(var, cloud, inliers_idx); 16 | 17 | // --- Compute the plane score --- 18 | double density_max = 150, score = 0.0; 19 | int inliers_min = 40; 20 | const int n = inliers_idx.rows(); 21 | if(n > inliers_min){ 22 | score = 80.0 + 0.2*(100.0 - (std::abs(density_max - n)) / 23 | double(std::max(density_max, double(n))) * 100.0); 24 | } 25 | 26 | // --- Set the score for this primitive --- 27 | this->setScore(score); 28 | 29 | } 30 | } 31 | 32 | /// ------- computeInliers() ------ 33 | Eigen::MatrixXi Plane::computeInliers(DecoratedCloud& cloud, double T, double alpha) { 34 | 35 | int numberPoint = cloud.getVertices().rows(); 36 | Eigen::MatrixXi inliers_idx(numberPoint, 1); 37 | 38 | Eigen::Matrix N = this->getNormal().normalized(); 39 | Eigen::Matrix P = this->getRefPoint(); 40 | 41 | 42 | if( N.norm() > 0 && numberPoint > 0) { 43 | const long n = cloud.getVertices().rows(); 44 | 45 | Eigen::Matrix _V, _N; 46 | 47 | int idx_counter = 0; double dist = 0; 48 | for (int i = 0; i < n; i++) { 49 | _V = cloud.getVertices().row(i); 50 | _N = cloud.getNormals().row(i).normalized(); 51 | if(_N.dot(N) < 0) _N = -_N; 52 | 53 | // --- Check if in range and if normals match --- 54 | dist = std::abs((N.dot(_V - P)) / N.norm()); 55 | if (dist < T && std::abs(_N.dot(N)) > alpha) { 56 | inliers_idx(idx_counter, 0) = i; 57 | idx_counter++; 58 | } 59 | } 60 | 61 | if (idx_counter == 0) inliers_idx = inliers_idx.topRows(1); 62 | else inliers_idx = inliers_idx.topRows(idx_counter - 1); 63 | } 64 | 65 | return inliers_idx ; 66 | } 67 | 68 | Primitive* Plane::clone(){ 69 | Primitive* thisPlane = new Plane(this->getRefPoint(), this->getNormal()) ; 70 | thisPlane->setType(2) ; 71 | return thisPlane; 72 | } 73 | } -------------------------------------------------------------------------------- /RANSAC/src/primitiveManager.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/impl/primitiveManager.hpp" 2 | #include 3 | 4 | namespace acq { 5 | 6 | // ******** This class is inspired from "cloudManager" 7 | 8 | void PrimitiveManager::addPrimitive(Primitive* primitive) { 9 | _primitives.push_back(primitive); 10 | } 11 | 12 | void PrimitiveManager::setPrimitive(Primitive* primitive, int index) { 13 | if (index >= _primitives.size()) { 14 | if (index != _primitives.size()) 15 | std::cerr << "[PrimitiveManager::setPrimitive] " 16 | << "Warning, creating " << index - _primitives.size() 17 | << " empty primitive when inserting to index " << index 18 | << ", current size is " << _primitives.size() 19 | << "...why not use addPrimitive?\n"; 20 | _primitives.resize(index + 1); 21 | } 22 | 23 | _primitives.at(index) = primitive; 24 | } 25 | 26 | Primitive* PrimitiveManager::getPrimitive(int index) { 27 | if (index < _primitives.size()) 28 | return _primitives.at(index); 29 | else { 30 | std::cerr << "Cannot return primitive with id " << index 31 | << ", only have " << _primitives.size() 32 | << " primitives ...returning empty primitive\n"; 33 | throw new std::runtime_error("No such primitive"); 34 | } 35 | } 36 | 37 | int PrimitiveManager::findBestScore() { 38 | int numberPrim = _primitives.size() ; 39 | double bestScore = 0 ; 40 | double thisScore ; 41 | int bestPrimIdx = 0 ; 42 | 43 | // go over all the primitives to find the best score 44 | for (int i=0; igetPrimitive(i) ; 47 | thisScore = thisPrim->getScore() ; 48 | 49 | // compare with previous result 50 | if (thisScore > bestScore) { 51 | // store it if best one 52 | bestPrimIdx = i ; 53 | bestScore = thisScore ; 54 | } 55 | } 56 | return bestPrimIdx ; 57 | } 58 | 59 | // delete the primitive at the position index 60 | void PrimitiveManager::deletePrimitive(int index) { 61 | delete this->getPrimitive(index); 62 | _primitives.erase(_primitives.begin() + index); 63 | } 64 | 65 | // delete all the primitives and the vector 66 | void PrimitiveManager::clearAllPrimitives() { 67 | std::vector::iterator thisPrimIt; 68 | 69 | for ( thisPrimIt = _primitives.begin(); thisPrimIt != _primitives.end(); ) { 70 | delete * thisPrimIt; 71 | thisPrimIt = _primitives.erase(thisPrimIt); 72 | } 73 | } 74 | 75 | void PrimitiveManager::deleteCloudFromIndex(int indexStart) { 76 | std::vector::iterator thisPrimitiveIt; 77 | 78 | for ( thisPrimitiveIt = _primitives.begin()+indexStart; thisPrimitiveIt != _primitives.end(); ) { 79 | thisPrimitiveIt = _primitives.erase(thisPrimitiveIt); 80 | } 81 | } 82 | 83 | 84 | } //...ns acq 85 | -------------------------------------------------------------------------------- /RANSAC/src/ransac.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/impl/ransac.hpp" 2 | 3 | namespace acq { 4 | 5 | bool ransac(DecoratedCloud &cloud, PrimitiveManager &best_primitives, CloudManager &cloudManager, 6 | double thresh, double alpha, double thresh_best, int iterationsTotal, int numberSample) { 7 | 8 | // ****************** INITIALISATION *********** 9 | int numberOfPoint = cloud.getVertices().rows() ; 10 | Eigen::Matrix thisSample ; 11 | Eigen::MatrixXi thisInliers ; 12 | bool prim_detected = false, test_thisSphere, test_thisPlane, test ; 13 | int bestPrim_idx, nbAllPrim, i ; 14 | double best_score ; 15 | int newSize = 4, n_inliers ; 16 | bool primitiveFound = false ; 17 | // compute the variance 18 | Eigen::Matrix3d variance = computeVariance(cloud.getVertices()) ; 19 | 20 | // will contain all the primitives created 21 | PrimitiveManager allPrimitive ; 22 | 23 | // create the primitive for this iteration 24 | for (i=0 ; i0) { 38 | // get back the best primitive and its score 39 | bestPrim_idx = allPrimitive.findBestScore() ; 40 | Primitive* best_prim = allPrimitive.getPrimitive(bestPrim_idx) ; 41 | best_score = best_prim->getScore() ; 42 | 43 | // keep it if good enough 44 | if (best_score > thresh_best) { 45 | // the inliers of this cloud 46 | thisInliers = best_prim->computeInliers(cloud, thresh, alpha) ; 47 | n_inliers = thisInliers.rows(); 48 | 49 | // extra test 50 | if(n_inliers > 1) { 51 | // copy the primitive to store and add it to the newCloud 52 | Primitive* prim_Storage = best_prim->clone() ; 53 | best_primitives.addPrimitive(prim_Storage) ; 54 | 55 | // clean the cloud and store the inliers in the cloud manager 56 | cleanCloud(cloud, cloudManager, thisInliers) ; 57 | numberOfPoint = cloud.getVertices().rows() ; 58 | primitiveFound = true ; 59 | } 60 | // clean the primitive 61 | allPrimitive.deletePrimitive(bestPrim_idx) ; 62 | } 63 | // if the primitive isn't good enough, not take into account 64 | else { 65 | allPrimitive.deletePrimitive(bestPrim_idx) ; 66 | } 67 | 68 | // if there is not enough points in the cloud 69 | if (numberOfPoint < 3) { 70 | break ; 71 | } 72 | } 73 | } 74 | // free the memory allocated with all the primitives not used 75 | allPrimitive.clearAllPrimitives() ; 76 | 77 | // cloudManager and cloudPrimitive contains the result of the function 78 | return primitiveFound ; // Just return a bool if ransac finds something 79 | }; 80 | 81 | } 82 | 83 | 84 | -------------------------------------------------------------------------------- /RANSAC/src/reconstruction.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/reconstruction.h" 2 | 3 | namespace acq { 4 | /// ---- RECONSTRUCTION ---- 5 | void reconstruct(PrimitiveManager& best_primitives, DecoratedCloud& cloud, int nbSamples, double T, double alpha, double T2) { 6 | const int n = best_primitives.getCloudSize(); 7 | for (int i = 0; i < n; i++) { // For every primitive 8 | 9 | if (best_primitives.getPrimitive(i)->getType() == 2) { // If we found a plane 10 | Primitive *this_prim = best_primitives.getPrimitive(i); 11 | Eigen::MatrixXi inliers_idx = static_cast(this_prim)->computeInliers(cloud, T, alpha); // Retrieve the inliers 12 | sampleFromPrimitive(cloud, inliers_idx, this_prim, nbSamples, T2); 13 | } 14 | } 15 | } 16 | 17 | 18 | 19 | void sampleFromPrimitive(DecoratedCloud& cloud, Eigen::MatrixXi inliers_idx, Primitive* plane, int nbSample, double T) { 20 | /* Compute the optimal number of points for this plane, given the cloud variance and the inliers */ 21 | 22 | Eigen::Matrix new_V, new_C, new_N, new_V_XY; 23 | 24 | // --- Find an orthonormal basis of the plane --- 25 | Eigen::Matrix N(1, 3); N = static_cast(plane)->getNormal().normalized(); 26 | Eigen::Matrix u(1, 3); 27 | 28 | // Compute u so that it's orthogonal to N 29 | if(N(0,1)!=0 || N(0,0)!=0) 30 | u << -N(0,1), N(0,0), 0.0; 31 | else if(N(0,2)!=0 || N(0,0)!=0) 32 | u << -N(0,2), 0.0, N(0,0); 33 | else u << 0.0, -N(0,2), N(0,1); 34 | if(u.norm()!=0.0) u.normalize(); 35 | 36 | // Compute v to make a basis (u,v,N) 37 | Eigen::MatrixXd v = u.row(0).cross(-N.row(0)); 38 | if(v.norm()!=0.0) v.normalize(); 39 | 40 | // --- Retrieve 3D Inliers and project on (u,v) basis --- 41 | const int n = inliers_idx.rows(); 42 | Eigen::MatrixXd inliers2D(n, 2); 43 | Eigen::MatrixXd this_vertex; 44 | int idx; 45 | 46 | std::cout << "OKK3" << std::endl; 47 | 48 | 49 | for(int i = 0; i(plane)->getRefPoint().dot(static_cast(plane)->getNormal().normalized())*static_cast(plane)->getNormal().normalized(); 107 | 108 | new_V.row(current_idx) = new_vertex.row(0); 109 | new_V_XY.row(current_idx) = new_vertex_XY.row(0); 110 | new_C.row(current_idx) = new_color.row(0); 111 | new_N.row(current_idx) = static_cast(plane)->getNormal().normalized(); 112 | 113 | current_idx++; 114 | 115 | } 116 | } 117 | 118 | 119 | if(current_idx > 0) { 120 | 121 | Eigen::MatrixXd V = new_V.topRows(current_idx); 122 | Eigen::MatrixXd C = new_C.topRows(current_idx); 123 | Eigen::MatrixXd N = new_N.topRows(current_idx); 124 | 125 | const int n_rows = current_idx + cloud.getVertices().rows(); 126 | Eigen::MatrixXd joined_V(n_rows, 3), joined_C(n_rows, 3), joined_N(n_rows, 3); 127 | 128 | joined_V << cloud.getVertices(), V; 129 | joined_C << cloud.getColors() , C; 130 | joined_N << cloud.getNormals() , N; 131 | 132 | cloud.setVertices(joined_V); 133 | cloud.setColors(joined_C); 134 | cloud.setNormals(joined_N); 135 | 136 | } 137 | } 138 | } 139 | 140 | 141 | -------------------------------------------------------------------------------- /RANSAC/src/sphere.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/primitive.h" 2 | 3 | namespace acq { 4 | void Sphere::computeScore(Eigen::Matrix3d variance, DecoratedCloud& cloud, double threshold, double alpha) { 5 | // compute the inliers 6 | Eigen::MatrixXi inliers_idx = this->computeInliers(cloud,threshold, alpha) ; 7 | 8 | // set the inliers for this primitive 9 | this->setInliers_idx(inliers_idx) ; 10 | 11 | // *** first try *** 12 | // Look how many point are supposed to be there 13 | //int bestNumber = this->findBestNumberPoints(variance) ; 14 | 15 | // *** new try *** 16 | int numberInliers = inliers_idx.rows() ; 17 | 18 | // choose set a score only if the primitive has at least 50 inliers 19 | double density_max = 70, score = 0; 20 | int inliers_min = 50; 21 | if(numberInliers > inliers_min){ 22 | score = 80 + 0.2*(100.0 - (std::abs(density_max - numberInliers)) / 23 | double(std::max(density_max, double(numberInliers))) * 100.0); 24 | } 25 | 26 | // set the score for this primitive 27 | this->setScore(score) ; 28 | } 29 | 30 | // find the best number of point for this sphere accordingly to the radius and the variance of points : first idea 31 | int Sphere::findBestNumberPoints(Eigen::Matrix3d variance) { 32 | double thisArea = M_PI*4.0*pow(_radius, 2.0) ; 33 | Eigen::Matrix varianceVector = variance.diagonal() ; 34 | 35 | double meanVariance = varianceVector.norm() ; 36 | double areaAroundPoint = M_PI*pow(meanVariance/4.7, 2.0) ; 37 | 38 | //double areaAroundPoint = M_PI*pow(meanVariance, 2.0) ; 39 | 40 | int numberPoints = floor(thisArea/areaAroundPoint) ; 41 | 42 | return numberPoints ; 43 | } 44 | 45 | // compute the inliers in a mesh 46 | Eigen::MatrixXi Sphere::computeInliers(DecoratedCloud& cloud, double threshold, double alpha) { 47 | int numberPoint = cloud.getVertices().rows() , index_inliers = 0; 48 | Eigen::Matrix thisVertice, thisNormal, estimatedNormal ; 49 | double thisRadius, test1, test2 ; 50 | Eigen::MatrixXi inliers_idx(numberPoint, 1) ; 51 | 52 | // test for each point if it is in the sphere or not 53 | for (int i=0; i < numberPoint; i++) { 54 | thisVertice = cloud.getVertices().row(i) ; 55 | thisNormal = cloud.getNormals().row(i) ; 56 | 57 | // compute the estimated normal and radius for this point 58 | thisRadius = (thisVertice - _center).norm() ; 59 | estimatedNormal = (thisVertice - _center).normalized() ; 60 | 61 | // test between the distance and the radius 62 | test1 = thisRadius - _radius ; 63 | test2 = estimatedNormal.dot(thisNormal) ; 64 | 65 | if (std::abs(test1) < threshold ) { 66 | if ( std::abs(test2) > alpha ) { 67 | // if the 2 test are true, the point is an inlier 68 | inliers_idx(index_inliers,0) = i ; 69 | index_inliers += 1 ; 70 | } 71 | } 72 | } 73 | 74 | // only get back the important part 75 | if (index_inliers == 0) { 76 | inliers_idx = inliers_idx.topRows(1); 77 | } 78 | else { 79 | inliers_idx = inliers_idx.topRows(index_inliers - 1); 80 | } 81 | return inliers_idx ; 82 | } 83 | 84 | // will clone a sphere with the right attributs when called 85 | Primitive* Sphere::clone(){ 86 | Primitive* thisSphere = new Sphere(this->getRadius(), this->getCenter()) ; 87 | thisSphere->setType(1) ; 88 | return thisSphere; 89 | } 90 | 91 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 3D geometry processing algorithms 2 | 3 | ## RANSAC 4 | 5 | Implementation of the RANSAC algorithm in 3D to detect primitives. The algorithm is based on this paper : «Efficient RANSAC for Point-Cloud Shape Detection» from Reinhard Klein, Ruwen Schnabel and Roland Wahl. 6 | 7 | The report and results are in the folder 8 | 9 | Student at UCL : ************** January 2017 - Mars 2017 *********** 10 | 11 | ## Smoothing 12 | 13 | Perform Laplacian 3D mesh smoothing using ANN librairy. 14 | 15 | Report in the folder 16 | 17 | Student at UCL : ************** January 2017 - Mars 2017 *********** 18 | 19 | ## ICP 20 | 21 | Perform ICP point to point and point to plane to a 3D mesh using ANN librairy 22 | The functions are written as class function in the decouratedCloud.cpp file, interface described in the main 23 | Implementation of ICP point to point and point to plane, use of CGAL to show intersections 24 | 25 | Report in the folder 26 | 27 | Student at UCL : ************** January 2017 - Mars 2017 *********** 28 | -------------------------------------------------------------------------------- /Smoothing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(IGLFramework) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | 6 | # Compile type: Release, Debug, RelWithDebInfo 7 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 8 | 9 | # Where external libraries are 10 | set(THIRD_PARTY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty) 11 | 12 | # definitions 13 | #add_definitions(-DIGL_VIEWER_WITH_NANOGUI -DNANOVG_GL3_IMPLEMENTATION) 14 | add_definitions(-DIGL_VIEWER_WITH_NANOGUI) 15 | 16 | 17 | # ################################################################ # 18 | # IGL 19 | # ################################################################ # 20 | 21 | include_directories(${THIRD_PARTY_DIR}/libigl/include) 22 | 23 | #include_directories(${THIRD_PARTY_DIR}/libigl/install/include) 24 | #link_directories(${THIRD_PARTY_DIR}/libigl/lib) 25 | 26 | # ################################################################ # 27 | # NanoGui 28 | # ################################################################ # 29 | 30 | # Disable building extras we won't need (pure C++ project) 31 | set(NANOGUI_BUILD_EXAMPLE OFF CACHE BOOL " " FORCE) 32 | set(NANOGUI_BUILD_PYTHON OFF CACHE BOOL " " FORCE) 33 | set(NANOGUI_INSTALL OFF CACHE BOOL " " FORCE) 34 | #set(NANOGUI_USE_GLAD OFF CACHE BOOL " " FORCE) 35 | #set(NANOGUI_USE_GLAD_DEFAULT OFF CACHE BOOL " " FORCE) 36 | 37 | # Add the configurations from nanogui 38 | add_subdirectory(${THIRD_PARTY_DIR}/libigl/external/nanogui) 39 | if (NOT WIN32) 40 | set_target_properties(nanogui-obj PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 41 | set_target_properties(nanogui PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 42 | endif() 43 | 44 | include_directories(${THIRD_PARTY_DIR}/libigl/external/nanogui/include) 45 | 46 | # For reliability of parallel build, make the NanoGUI targets dependencies 47 | set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies") 48 | 49 | # Various preprocessor definitions have been generated by NanoGUI 50 | add_definitions(${NANOGUI_EXTRA_DEFS}) 51 | MESSAGE(STATUS "NANOGUI_EXTRA_DEFS ${NANOGUI_EXTRA_DEFS}") 52 | # prints: -DNANOGUI_SHARED;-DNVG_SHARED;-DGLAD_GLAPI_EXPORT 53 | 54 | # On top of adding the path to nanogui/include, you may need extras 55 | include_directories(${NANOGUI_EXTRA_INCS}) 56 | MESSAGE(STATUS "NANOGUI_EXTRA_INCS: ${NANOGUI_EXTRA_INCS}") 57 | # prints: \ 58 | # 3rdparty/libigl/external/nanogui/ext/glfw/include; \ 59 | # 3rdparty/libigl/external/nanogui/ext/nanovg/src; \ 60 | # 3rdparty/libigl/external/nanogui/ext/eigen 61 | 62 | MESSAGE(STATUS "NANOGUI_EXTRA_LIBS: ${NANOGUI_EXTRA_LIBS}") 63 | # prints: GL;Xxf86vm;Xrandr;Xinerama;Xcursor;Xi;X11;pthread;rt;dl 64 | 65 | # ################################################################ # 66 | # Eigen 67 | # ################################################################ # 68 | 69 | if (DEFINED Eigen3_DIR) 70 | # use system version 71 | set(Eigen3_DIR ${THIRD_PARTY_DIR}/eigen) 72 | MESSAGE(STATUS "Eigen3_DIR: ${Eigen3_DIR}") 73 | find_package(Eigen3 REQUIRED HINTS ${Eigen3_DIR}) 74 | else() 75 | # use libigl's version 76 | set(EIGEN3_INCLUDE_DIR ${THIRD_PARTY_DIR}/libigl/external/nanogui/ext/eigen) 77 | endif() 78 | include_directories(${EIGEN3_INCLUDE_DIR}) 79 | MESSAGE(STATUS "EIGEN3_INCLUDE_DIR: ${EIGEN3_INCLUDE_DIR}") 80 | 81 | # ################################################################ # 82 | # NanoFLANN 83 | # ################################################################ # 84 | include_directories(${THIRD_PARTY_DIR}/nanoflann/include) 85 | 86 | # ################################################################ # 87 | # GLEW 88 | # ################################################################ # 89 | 90 | #if (WIN32) 91 | # set(GLEW_DIR "${THIRD_PARTY_DIR}/glew-2.0.0") 92 | # if (NOT EXISTS GLEW_DIR) 93 | # MESSAGE(FATAL Glew directory does not exist: ${GLEW_DIR}) 94 | # endif() 95 | # set(GLEW_LIBRARIES "glew32s") 96 | # set(GLEW_INCLUDE_DIRS "${GLEW_DIR}/include") 97 | # link_libraries(${GLEW_DIR}/lib/Release/x64) 98 | #else() 99 | if (NOT WIN32) 100 | set(GLEW_STATIC) 101 | find_package(GLEW REQUIRED) 102 | if (NOT DEFINED GLEW_LIBRARIES) 103 | MESSAGE(FATAL "GLEW LIBRARIES variable not filled...") 104 | endif() 105 | endif() 106 | include_directories(${GLEW_INCLUDE_DIRS}) 107 | 108 | # ################################################################ # 109 | # OPENMESH 110 | # ################################################################ # 111 | 112 | include_directories ( 113 | /Users/Maud/Documents/OpenMesh-6.3/built${CMAKE_CURRENT_SOURCE_DIR} 114 | ) 115 | link_directories(/Users/Maud/Documents/OpenMesh-6.3/built/Build/lib) 116 | 117 | # ################################################################ # 118 | # Project 119 | # ################################################################ # 120 | 121 | include_directories(include) 122 | 123 | # List of source files 124 | set(SOURCE_FILES 125 | src/main.cpp 126 | include/acq/typedefs.h 127 | include/acq/normalEstimation.h 128 | include/acq/discreteCurvature.h 129 | include/acq/impl/normalEstimation.hpp 130 | include/acq/impl/discreteCurvature.hpp 131 | include/acq/impl/smoothing.hpp 132 | 133 | src/normalEstimation.cpp 134 | src/smoothing.cpp 135 | src/discreteCurvature.cpp 136 | include/acq/impl/decoratedCloud.hpp 137 | include/acq/decoratedCloud.h 138 | include/acq/cloudManager.h 139 | include/acq/smoothing.h 140 | include/acq/impl/cloudManager.hpp 141 | src/decoratedCloud.cpp 142 | src/cloudManager.cpp 143 | ) 144 | 145 | # Create program 146 | add_executable(iglFramework ${SOURCE_FILES}) 147 | 148 | # Add defines 149 | target_compile_definitions(iglFramework PUBLIC -DNANOVG_GL3_IMPLEMENTATION) 150 | if (WIN32) 151 | target_compile_definitions(iglFramework PUBLIC -DGLFW_INCLUDE_NONE -DNDEBUG -D_CONSOLE -D_USE_MATH_DEFINES -D_CRT_SECURE_NO_WARNINGS -DGLEW_STATIC) 152 | endif() 153 | 154 | if (NOT WIN32) 155 | set_target_properties(iglFramework PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) 156 | endif() 157 | 158 | # Link dependency libraries 159 | target_link_libraries(iglFramework 160 | nanogui 161 | ${NANOGUI_EXTRA_LIBS} 162 | ${GLEW_LIBRARIES} 163 | OpenMeshCore 164 | OpenMeshTools 165 | ) 166 | 167 | if (WIN32) 168 | add_custom_command(TARGET iglFramework POST_BUILD 169 | COMMAND ${CMAKE_COMMAND} -E 170 | copy_if_different "${CMAKE_CURRENT_BINARY_DIR}/3rdparty/libigl/external/nanogui/${CMAKE_BUILD_TYPE}/nanogui.dll" 171 | $/nanogui.dll) 172 | endif() 173 | 174 | # Untested! 175 | # Optional to compile IGL: http://libigl.github.io/libigl/optional/ 176 | # cd libigl 177 | # mkdir lib 178 | # cd lib 179 | # cmake -DCMAKE_BUILD_TYPE=Release -DLIBIGL_WITH_NANOGUI=YES -DLIBIGL_WITH_ANTTWEAKBAR=NO -DLIBIGL_WITH_CGAL=NO ../optional 180 | 181 | -------------------------------------------------------------------------------- /Smoothing/coursework2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mbuffier/3D_Geometry_Processing/61807fdd0962addcd1907c246e9225876a5292c5/Smoothing/coursework2.pdf -------------------------------------------------------------------------------- /Smoothing/include/acq/cloudManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_CLOUDMANAGER_H 6 | #define ACQ_CLOUDMANAGER_H 7 | 8 | #include "acq/decoratedCloud.h" 9 | #include 10 | 11 | namespace acq { 12 | 13 | /** \brief Small class to keep track of multiple point clouds. */ 14 | class CloudManager { 15 | public: 16 | /** \brief Append a cloud to the list of clouds. */ 17 | void addCloud(DecoratedCloud const& cloud); 18 | 19 | /** \brief Overwrite a cloud at a specific index. */ 20 | void setCloud(DecoratedCloud const& cloud, int index); 21 | 22 | /** \brief Get cloud with specific index. */ 23 | DecoratedCloud& getCloud(int index); 24 | 25 | /** \brief Get cloud with specific index (const version). */ 26 | DecoratedCloud const& getCloud(int index) const; 27 | 28 | protected: 29 | std::vector _clouds; //!< List of clouds possibly with normals and faces. 30 | 31 | public: 32 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | }; //...class CloudManager 35 | 36 | } //...ns acq 37 | 38 | #endif // ACQ_CLOUDMANAGER_H -------------------------------------------------------------------------------- /Smoothing/include/acq/decoratedCloud.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_DECORATEDCLOUD_H 6 | #define ACQ_DECORATEDCLOUD_H 7 | 8 | #include "acq/typedefs.h" 9 | 10 | namespace acq { 11 | 12 | /** \brief Simple class to keep track of points normals and faces for a point cloud or mesh. */ 13 | class DecoratedCloud { 14 | public: 15 | /** \brief Default constructor leaving fields empty. */ 16 | explicit DecoratedCloud() {} 17 | 18 | /** \brief Constructor filling point information only. */ 19 | explicit DecoratedCloud(CloudT const& vertices); 20 | 21 | /** \brief Constructor filling point and face information. */ 22 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces); 23 | 24 | /** \brief Constructor filling point and normal information. */ 25 | explicit DecoratedCloud(CloudT const& vertices, NormalsT const& normals); 26 | 27 | /** \brief Constructor filling point, face and normal information. */ 28 | explicit DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals); 29 | 30 | /** \brief Getter for point cloud. */ 31 | CloudT const& getVertices() const { return _vertices; } 32 | /** \brief Setter for point cloud. */ 33 | void setVertices(CloudT const& vertices) { _vertices = vertices; } 34 | /** \brief Check, if any points stored. */ 35 | bool hasVertices() const { return static_cast(_vertices.size()); } 36 | 37 | /** \brief Getter for face indices list. */ 38 | FacesT const& getFaces() const { return _faces; } 39 | /** \brief Setter for face indices list. */ 40 | void setFaces(FacesT const& faces) { _faces = faces; } 41 | /** \brief Check, if any faces stored. */ 42 | bool hasFaces() const { return static_cast(_faces.size()); } 43 | 44 | /** \brief Getter for normals. */ 45 | NormalsT & getNormals() { return _normals; } 46 | /** \brief Getter for normals (const version). */ 47 | NormalsT const& getNormals() const { return _normals; } 48 | /** \brief Setter for normals. */ 49 | void setNormals(NormalsT const& normals) { _normals = normals; } 50 | /** \brief Check, if any normals stored. */ 51 | bool hasNormals() const { return static_cast(_normals.size()); } 52 | 53 | ColorsT const& getColors() const { return _colors; } 54 | 55 | 56 | protected: 57 | CloudT _vertices; //!< Point cloud, N x 3 matrix where N is the number of points. 58 | FacesT _faces; //!< Faces stored as rows of vertex indices (referring to \ref _vertices). 59 | NormalsT _normals; //!< Per-vertex normals, associated with \ref _vertices by row ID. 60 | ColorsT _colors ; 61 | 62 | public: 63 | // See https://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html 64 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 65 | }; //...class DecoratedCloud() 66 | 67 | } //...ns acq 68 | 69 | #endif //ACQ_DECORATEDCLOUD_H -------------------------------------------------------------------------------- /Smoothing/include/acq/discreteCurvature.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DISCRETECURVATURE_H 3 | #define DISCRETECURVATURE_H 4 | 5 | #include "acq/decoratedCloud.h" 6 | 7 | #include 8 | #include 9 | 10 | #include "igl/jet.h" 11 | #include "igl/unique.h" 12 | #include "igl/triangle_triangle_adjacency.h" 13 | 14 | #define _USE_MATH_DEFINES 15 | 16 | namespace acq { 17 | 18 | // ************* function to find area and angle of a triangle ************** 19 | double computeArea(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) ; 20 | double computeAngle(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) ; 21 | double computeCotan(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) ; 22 | 23 | // ************* Function which return the L matrix for a mesh ************** 24 | // uniform discretization 25 | Eigen::SparseMatrix uniformLaplacian(DecoratedCloud & cloud) ; 26 | 27 | // cotan discretization 28 | Eigen::SparseMatrix diagonalArea(DecoratedCloud & cloud) ; 29 | Eigen::SparseMatrix weightCotan(DecoratedCloud & cloud) ; 30 | Eigen::SparseMatrix inverseSparse(Eigen::SparseMatrix M) ; 31 | 32 | Eigen::SparseMatrix computeCotanDiscretization(DecoratedCloud & cloud) ; 33 | 34 | // ************* Functions for curvature estimation ************** 35 | Eigen::MatrixXd meanCurvature(DecoratedCloud & cloud, int typeDiscretization) ; 36 | Eigen::MatrixXd gaussianCurvUnifom(DecoratedCloud & cloud) ; 37 | } 38 | #endif -------------------------------------------------------------------------------- /Smoothing/include/acq/impl/cloudManager.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_CLOUDMANAGER_HPP 6 | #define ACQ_CLOUDMANAGER_HPP 7 | 8 | #include "acq/cloudManager.h" 9 | 10 | namespace acq { 11 | 12 | } //...ns acq 13 | 14 | #endif //ACQ_CLOUDMANAGER_HPP 15 | -------------------------------------------------------------------------------- /Smoothing/include/acq/impl/decoratedCloud.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_DECORATEDCLOUD_HPP 6 | #define ACQ_DECORATEDCLOUD_HPP 7 | 8 | #include "acq/decoratedCloud.h" 9 | 10 | namespace acq { 11 | 12 | } //...ns acq 13 | 14 | #endif //ACQ_DECORATEDCLOUD_HPP 15 | -------------------------------------------------------------------------------- /Smoothing/include/acq/impl/discreteCurvature.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_DISCRETECURVATURE_HPP 6 | #define ACQ_DISCRETECURVATURE_HPP 7 | 8 | #include "acq/discreteCurvature.h" 9 | 10 | namespace acq { 11 | 12 | } //...ns acq 13 | 14 | #endif //ACQ_DECORATEDCLOUD_HPP 15 | -------------------------------------------------------------------------------- /Smoothing/include/acq/impl/normalEstimation.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Aron Monszpart on 19/01/17. 3 | // 4 | 5 | #ifndef ACQ_NORMALESTIMATION_HPP 6 | #define ACQ_NORMALESTIMATION_HPP 7 | 8 | #include "acq/normalEstimation.h" 9 | 10 | #include "Eigen/Eigenvalues" // SelfAdjointEigenSolver 11 | 12 | #include 13 | 14 | namespace acq { 15 | 16 | template 17 | Eigen::Matrix 18 | calculatePointNormal( 19 | CloudT const& cloud, // N x 3 20 | int const pointIndex, 21 | _NeighbourIdListT const& neighbourIndices 22 | ) { 23 | //! Floating point type 24 | typedef typename CloudT::Scalar Scalar; 25 | //! 3x1 vector type 26 | typedef Eigen::Matrix Vector3; 27 | //! 3x3 matrix type 28 | typedef Eigen::Matrix Matrix3; 29 | 30 | // Covariance matrix initialized to 0-s: 31 | Matrix3 cov(Matrix3::Zero()); 32 | 33 | // For each neighbouring point 34 | for (auto const neighbourIndex : neighbourIndices) { 35 | 36 | // Skip, if first neighbour is same point 37 | if (pointIndex == neighbourIndex) { 38 | continue; 39 | } //...if same index 40 | 41 | // Calculate relative vector to neighbour 42 | Vector3 const vToNeighbour = 43 | cloud.row(neighbourIndex) - cloud.row(pointIndex); 44 | 45 | // Sum up covariance 46 | cov += vToNeighbour * vToNeighbour.transpose(); 47 | 48 | } //...For neighbours 49 | 50 | // Solve for neighbourhood smallest eigen value 51 | Eigen::SelfAdjointEigenSolver es(cov); 52 | 53 | // Find index of smallest eigen value 54 | int const smallestEigenValueId = // 0, 1 2 55 | static_cast( 56 | std::distance( 57 | es.eigenvalues().data(), 58 | std::min_element( 59 | es.eigenvalues().data(), 60 | es.eigenvalues().data() + cloud.cols() 61 | ) 62 | ) 63 | ); 64 | 65 | // Return smallest eigen vector 66 | return es.eigenvectors() 67 | .col(smallestEigenValueId) 68 | .normalized(); 69 | 70 | } // calculatePointNormal() 71 | 72 | template 73 | NeighboursT 74 | calculateCloudNeighboursFromFaces( 75 | _FacesT const& faces 76 | ) { 77 | // calculate neighbours from faces' edge lists 78 | NeighboursT neighbours; 79 | // for each face 80 | for (int row = 0; row != faces.rows(); ++row) { 81 | // for each face vertex 82 | for (int vxId = 0; vxId != faces.cols(); ++vxId) { 83 | // id of "incoming" edge's start vertex 84 | int const leftNeighbourId = 85 | (vxId != 0) ? vxId - 1 86 | : faces.cols() - 1; 87 | // id of "outgoing" edge's end vertex 88 | int const rightNeighbourId = 89 | (vxId < faces.cols() - 1) ? vxId + 1 90 | : 0; 91 | // store vertex has left neighbour as neighbour 92 | neighbours[faces(row, vxId)].insert( 93 | faces(row, leftNeighbourId) 94 | ); 95 | // store left neighbour has vertex as neighbour 96 | neighbours[faces(row, leftNeighbourId)].insert( 97 | faces(row, vxId) 98 | ); 99 | // store vertex has right neighbour as neighbour 100 | neighbours[faces(row, vxId)].insert( 101 | faces(row, rightNeighbourId) 102 | ); 103 | // store right neighbour has vertex as neighbour 104 | neighbours[faces(row, rightNeighbourId)].insert( 105 | faces(row, vxId) 106 | ); 107 | } //...for each vertex in face 108 | } //...for each face 109 | 110 | return neighbours; 111 | } //...orientCloudNormalsFromFaces() 112 | 113 | template 114 | int orientCloudNormalsFromFaces( 115 | _FacesT const& faces, 116 | _NormalsT & normals 117 | ) { 118 | return orientCloudNormals( 119 | calculateCloudNeighboursFromFaces(faces), 120 | normals 121 | ); 122 | } //...orientCloudNormalsFromFaces() 123 | 124 | } //...ns acq 125 | 126 | #endif //ACQ_NORMALESTIMATION_HPP 127 | -------------------------------------------------------------------------------- /Smoothing/include/acq/impl/smoothing.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #ifndef ACQ_SMOOTHING_HPP 6 | #define ACQ_SMOOTHING_HPP 7 | 8 | #include "acq/smoothing.h" 9 | 10 | namespace acq { 11 | 12 | } //...ns acq 13 | 14 | #endif //ACQ_SMOOTHING_HPP 15 | -------------------------------------------------------------------------------- /Smoothing/include/acq/normalEstimation.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Aron Monszpart on 19/01/17. 3 | // 4 | 5 | #ifndef ACQ_NORMALESTIMATION_H 6 | #define ACQ_NORMALESTIMATION_H 7 | 8 | #include "acq/typedefs.h" 9 | #include 10 | #include 11 | 12 | namespace acq { 13 | 14 | /** \addtogroup NormalEstimation 15 | * @{ 16 | */ 17 | 18 | /** \brief Estimates the normal of a single point 19 | * given its ID and the ID of its neighbours. 20 | * 21 | * \param[in] cloud N x 3 matrix containing points in rows. 22 | * \param[in] pointIndex Row-index of point. 23 | * \param[in] neighbourIndices List of row-indices of neighbours. 24 | * 25 | * \return A 3D vector that is the normal of point with ID \p pointIndex. 26 | */ 27 | template 28 | Eigen::Matrix 29 | calculatePointNormal( 30 | CloudT const& cloud, 31 | int const pointIndex, 32 | _NeighbourIdListT const& neighbourIndices); 33 | 34 | 35 | /** \brief Estimates the neighbours of all points in cloud 36 | * returning \p k neighbours max each. 37 | * 38 | * \param[in] k How many neighbours too look for in point. 39 | * \param[in] maxDist Maximum distance between vertex and neighbour. 40 | * \param[in] maxLeafs FLANN parameter, maximum kdTree depth. 41 | * 42 | * \return An associative container with the varying length lists of neighbours. 43 | */ 44 | NeighboursT 45 | calculateCloudNeighbours( 46 | CloudT const& cloud, 47 | int const k, 48 | float const maxDist = std::sqrt(std::numeric_limits::max()) - 1.f, 49 | int const maxLeafs = 10); 50 | 51 | /** \brief Estimates the normals of all points in cloud using \p k neighbours max each. 52 | * 53 | * \param[in] cloud Input pointcloud, N x 3, N 3D points in rows. 54 | * \param[in] neighbours Precomputed lists of neighbour Ids. 55 | * 56 | * \return N x 3 3D normals, the normals of the points in \p cloud. 57 | */ 58 | NormalsT 59 | calculateCloudNormals( 60 | CloudT const& cloud, 61 | NeighboursT const& neighbours); 62 | 63 | /** \brief Breadth-first-search to orient normals consistently 64 | * using the provided neighbourhood information. 65 | * 66 | * \param[in] neighbours A directed list of neighbour indices. 67 | * \param[in,out] normals The normals to possibly flip. 68 | * 69 | * \return The number of normals flipped. 70 | */ 71 | int 72 | orientCloudNormals( 73 | NeighboursT const& neighbours, 74 | NormalsT & normals); 75 | 76 | /** \brief Traverses faces and records neighbourhood information using face edges. 77 | * 78 | * \tparam _FacesT Concept: acq::FacesT aka. Eigen::MatrixXi. 79 | * 80 | * \param faces Indices of vertices belonging to a face in each row. 81 | * 82 | * \return The directed neighbourhood information. 83 | */ 84 | template 85 | NeighboursT 86 | calculateCloudNeighboursFromFaces( 87 | _FacesT const& faces 88 | ); 89 | 90 | /** \brief Estimates neighbourhood information from faces, 91 | * and then consistently flips normals using BFS traversal. 92 | * 93 | * \tparam _NormalsT Concept: acq::NormalsT, aka. Eigen::MatrixXd. 94 | * \tparam _FacesT Concept: acq::FacesT, aka. Eigen::MatrixXi. 95 | * 96 | * \param[in ] faces Face vertex indices in rows. 97 | * \param[in,out] normals The normals to possibly flip. 98 | * \return 99 | */ 100 | template 101 | int 102 | orientCloudNormalsFromFaces( 103 | _FacesT const& faces, 104 | _NormalsT & normals); 105 | 106 | /** @} (NormalEstimation) */ 107 | 108 | } //...ns acq 109 | 110 | #endif //ACQ_NORMALESTIMATION_H 111 | -------------------------------------------------------------------------------- /Smoothing/include/acq/smoothing.h: -------------------------------------------------------------------------------- 1 | #ifndef SMOOTHING_H 2 | #define SMOOTHING_H 3 | 4 | #include "acq/decoratedCloud.h" 5 | #include "acq/discreteCurvature.h" 6 | 7 | // eigen solver 8 | #include 9 | #include 10 | #include 11 | 12 | // IGL function used 13 | #include "igl/jet.h" 14 | #include "igl/unique.h" 15 | #include "igl/triangle_triangle_adjacency.h" 16 | #include "igl/bounding_box_diagonal.h" 17 | 18 | namespace acq { 19 | // ******* function to help to perform smoothing ******** 20 | double findLambda(double pourcentage, DecoratedCloud &cloud) ; 21 | 22 | /// ******* function to produce noise to perform smoothing ******** 23 | Eigen::MatrixXd addNoise(float noise, DecoratedCloud& cloud) ; 24 | void computeBoundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin, DecoratedCloud& cloud); 25 | 26 | // *********$ Function to perform smoothing ********* 27 | Eigen::MatrixXd explicitSmoothing(DecoratedCloud &cloud, double lambda, bool typeDiscretization) ; 28 | Eigen::MatrixXd implicitSmoothing(DecoratedCloud& cloud, double lambda) ; 29 | 30 | // *********$ Function to test denoising ********* 31 | float computeError(DecoratedCloud &cloud1, DecoratedCloud &cloud2) ; 32 | } 33 | #endif -------------------------------------------------------------------------------- /Smoothing/include/acq/typedefs.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Aron Monszpart on 19/01/17. 3 | // 4 | 5 | #ifndef ACQ_TYPEDEFS_H 6 | #define ACQ_TYPEDEFS_H 7 | 8 | #include "Eigen/Core" 9 | 10 | // Use this, if using aligned vector types from Eigen 11 | // See also 12 | // http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html 13 | 14 | //#include "Eigen/StdVector" 15 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f) 16 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) 17 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f) 18 | //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4d) 19 | 20 | #include 21 | #include 22 | 23 | namespace acq { 24 | 25 | //! Dynamically sized matrix of points in rows, a "point cloud". 26 | typedef Eigen::MatrixXd CloudT; 27 | //! Dynamically sized matrix of vectors in rows, a list of normals. 28 | typedef Eigen::MatrixXd NormalsT; 29 | //! Dynamically sized matrix of face vertex indices in rows. 30 | typedef Eigen::MatrixXi FacesT; 31 | 32 | typedef Eigen::MatrixXd ColorsT ; 33 | 34 | 35 | /** \brief An associative storage of neighbour indices for point cloud 36 | * { pointId => [neighbourId_0, nId_1, ... nId_k-1] } 37 | */ 38 | typedef std::map > NeighboursT; 39 | 40 | } //...ns acq 41 | 42 | #endif //ACQ_TYPEDEFS_H 43 | -------------------------------------------------------------------------------- /Smoothing/src/cloudManager.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/cloudManager.hpp" 6 | #include 7 | 8 | namespace acq { 9 | 10 | void CloudManager::addCloud(DecoratedCloud const& cloud) { 11 | _clouds.push_back(cloud); 12 | } //...CloudManager::addCloud() 13 | 14 | void CloudManager::setCloud(DecoratedCloud const& cloud, int index) { 15 | if (index >= _clouds.size()) { 16 | if (index != _clouds.size()) 17 | std::cerr << "[CloudManager::setCloud] " 18 | << "Warning, creating " << index - _clouds.size() 19 | << " empty clouds when inserting to index " << index 20 | << ", current size is " << _clouds.size() 21 | << "...why not use addCloud?\n"; 22 | _clouds.resize(index + 1); 23 | } 24 | 25 | _clouds.at(index) = cloud; 26 | } //...CloudManager::setCloud() 27 | 28 | DecoratedCloud& CloudManager::getCloud(int index) { 29 | if (index < _clouds.size()) 30 | return _clouds.at(index); 31 | else { 32 | std::cerr << "Cannot return cloud with id " << index 33 | << ", only have " << _clouds.size() 34 | << " clouds...returning empty cloud\n"; 35 | throw new std::runtime_error("No such cloud"); 36 | } 37 | } //...CloudManager::getCloud() 38 | 39 | DecoratedCloud const& CloudManager::getCloud(int index) const { 40 | return const_cast( 41 | const_cast(this)->getCloud(index) 42 | ); 43 | } //...CloudManager::getCloud() (const) 44 | 45 | } //...ns acq -------------------------------------------------------------------------------- /Smoothing/src/decoratedCloud.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by bontius on 20/01/17. 3 | // 4 | 5 | #include "acq/impl/decoratedCloud.hpp" 6 | 7 | namespace acq { 8 | 9 | DecoratedCloud::DecoratedCloud(CloudT const& vertices) 10 | : _vertices(vertices) {} 11 | 12 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces) 13 | : _vertices(vertices), _faces(faces) 14 | {} 15 | 16 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, FacesT const& faces, NormalsT const& normals) 17 | : _vertices(vertices), _faces(faces), _normals(normals) 18 | {} 19 | 20 | DecoratedCloud::DecoratedCloud(CloudT const& vertices, NormalsT const& normals) 21 | : _vertices(vertices), _normals(normals) 22 | {} 23 | 24 | } //...ns acq -------------------------------------------------------------------------------- /Smoothing/src/discreteCurvature.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/discreteCurvature.h" 2 | 3 | namespace acq { 4 | 5 | // ******************* Functions to compute angle and area *************** 6 | 7 | double Pi = 3.14159265359 ; 8 | 9 | double computeArea(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) { 10 | Eigen::Vector3d vec1 = P2-P1 ; 11 | Eigen::Vector3d vec2 = P2-P3 ; 12 | Eigen::Vector3d vec3 = P3-P1 ; 13 | 14 | double side1 = vec1.norm() ; 15 | double side2 = vec2.norm() ; 16 | double side3 = vec3.norm() ; 17 | 18 | double halfP = (side1+side2+side3)/2.0 ; 19 | 20 | double area_value = sqrt(halfP*(halfP-side1)*(halfP-side2)*(halfP-side3)) ; 21 | return area_value ; 22 | } 23 | 24 | double computeAngle(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) { 25 | Eigen::Vector3d vec1 = P2-P1 ; 26 | Eigen::Vector3d vec2 = P3-P1 ; 27 | 28 | double norm1 = vec1.norm() ; 29 | double norm2 = vec2.norm() ; 30 | 31 | double angle = acos(vec1.dot(vec2)/(norm1*norm2)) ; 32 | return angle ; 33 | } 34 | 35 | double computeCotan(Eigen::Vector3d &P1, Eigen::Vector3d &P2, Eigen::Vector3d &P3) { 36 | Eigen::Vector3d vec1 = P2-P1 ; 37 | Eigen::Vector3d vec2 = P3-P1 ; 38 | 39 | double norm1 = vec1.norm() ; 40 | double norm2 = vec2.norm() ; 41 | 42 | double angle = acos(vec1.dot(vec2)/(norm1*norm2)) ; 43 | double cotan = tan(M_PI_2-angle) ; 44 | return cotan ; 45 | } 46 | 47 | // ************* Function which return the L matrix for a mesh ************** 48 | //************* uniform discretization ************** 49 | 50 | Eigen::SparseMatrix uniformLaplacian(DecoratedCloud & cloud) { 51 | // initialization of matrices of faces, vertices and normals 52 | int numbVertices = cloud.getVertices().rows() ; 53 | Eigen::MatrixXd normals(numbVertices, 3) ; 54 | Eigen::MatrixXd vertices(numbVertices, 3) ; 55 | Eigen::MatrixXi faces(numbVertices, 3) ; 56 | normals = cloud.getNormals() ; 57 | vertices = cloud.getVertices() ; 58 | faces = cloud.getFaces() ; 59 | 60 | // save the memory for the resulting matrix 61 | std::vector> tripletList; 62 | tripletList.reserve(8*numbVertices); 63 | Eigen::SparseMatrix L(numbVertices,numbVertices); 64 | 65 | // initialization for the function 66 | std::vector neighborIndexUnique ; 67 | bool findThisVer ; 68 | int position, valence ; 69 | float test ; 70 | 71 | // go through all the vertices of the mesh 72 | for (int i=0; i < vertices.rows(); i++) { 73 | 74 | // go through all the face of the mesh 75 | for (int j=0 ; j < faces.rows() ; j++ ) { 76 | findThisVer = false ; 77 | 78 | // go throught the vertices of the face of find the current one 79 | for (int k = 0 ; k < 3 ; k++) { 80 | if (faces(j,k) == i) { 81 | position = k ; 82 | findThisVer = true ; 83 | break ; 84 | } 85 | } 86 | 87 | // push back the neighbor vertex (all the faces in the same sens) 88 | if (findThisVer) { 89 | neighborIndexUnique.push_back(faces(j,(position +1)%3)) ; 90 | } 91 | } 92 | 93 | valence = neighborIndexUnique.size() ; 94 | 95 | // fill in the matrix with the found neighboring vertices 96 | for (int neigh =0; neigh < valence ; neigh++) { 97 | tripletList.push_back(Eigen::Triplet(i, neighborIndexUnique[neigh],1.0/double(valence))); 98 | } 99 | 100 | tripletList.push_back(Eigen::Triplet(i, i,-1)); 101 | neighborIndexUnique.clear() ; 102 | } 103 | L.setFromTriplets(tripletList.begin(), tripletList.end()); 104 | 105 | return L ; 106 | } 107 | 108 | //************* cotan discretization ************** 109 | 110 | Eigen::SparseMatrix diagonalArea(DecoratedCloud & cloud) { 111 | // initialization 112 | int numbVertices = cloud.getVertices().rows() ; 113 | Eigen::MatrixXd vertices(numbVertices, 3) ; 114 | Eigen::MatrixXi faces(numbVertices, 3) ; 115 | vertices = cloud.getVertices() ; 116 | faces = cloud.getFaces() ; 117 | 118 | // reserve for the triplet list 119 | std::vector> tripletList; 120 | tripletList.reserve(numbVertices); 121 | Eigen::SparseMatrix M(numbVertices,numbVertices); 122 | 123 | float thisArea, areaTotal ; 124 | bool findThisVer ; 125 | int position ; 126 | Eigen::Vector3d P1, P2, P3 ; 127 | 128 | // loop over the vertices 129 | for (int i=0; i < vertices.rows(); i++) { 130 | areaTotal = 0.0 ; 131 | // loop over the faces 132 | for (int j=0 ; j < faces.rows() ; j++ ) { 133 | findThisVer = false ; 134 | for (int k = 0 ; k < 3 ; k++) { 135 | if (faces(j,k) == i) { 136 | position = k ; 137 | findThisVer = true ; 138 | break ; 139 | } 140 | } 141 | 142 | // if the vertex is in the faces, compute the area of the triangle 143 | if (findThisVer) { 144 | // the vertices of this triangle 145 | P1 = vertices.row(i) ; 146 | P2 = vertices.row(faces(j,(position +1)%3)) ; 147 | P3 = vertices.row(faces(j,(position +2)%3)) ; 148 | 149 | // compute the angle for the triangle and the area 150 | thisArea = computeArea(P1, P2, P3) ; 151 | areaTotal += thisArea ; 152 | } 153 | thisArea = 0.0 ; 154 | } 155 | // compute the area for this vertex as 1/3 of the total area of the triangles 156 | areaTotal = areaTotal/3.0 ; 157 | // add the diagonal term 158 | tripletList.push_back(Eigen::Triplet(i, i,2.0*areaTotal)); 159 | } 160 | M.setFromTriplets(tripletList.begin(), tripletList.end()); 161 | return M ; 162 | } 163 | 164 | 165 | Eigen::SparseMatrix weightCotan(DecoratedCloud & cloud) { 166 | // initialization 167 | int numbVertices = cloud.getVertices().rows() ; 168 | 169 | // faces, normals anf vertices 170 | Eigen::MatrixXd vertices(numbVertices, 3) ; 171 | Eigen::MatrixXi faces(numbVertices, 3) ; 172 | vertices = cloud.getVertices() ; 173 | faces = cloud.getFaces() ; 174 | 175 | std::vector> tripletList; 176 | tripletList.reserve(10*numbVertices); // more places than needed in case 177 | Eigen::SparseMatrix C(numbVertices,numbVertices); 178 | 179 | // compute the adjacency informations for each triangle 180 | Eigen::MatrixXd TT(numbVertices,3) ; 181 | Eigen::MatrixXd TTi(numbVertices,3) ; 182 | igl::triangle_triangle_adjacency(faces, TT, TTi); 183 | 184 | // contains the information of position and weight for 1 neighbor 185 | std::vector thisNeighborInfo ; 186 | thisNeighborInfo.reserve(2) ; 187 | // contains the information for all the neighbors 188 | std::vector> neighborInformation ; 189 | neighborInformation.reserve(10) ; 190 | 191 | bool findThisVer ; 192 | double sumWeight, weight, thisWeight; 193 | int position, thisNeight, triangle2, edge2, valence ; 194 | Eigen::Vector3d PCurrent, PNeighCurrent, PTriangle1, PTriangle2 ; 195 | 196 | // loop over the vertices 197 | for (int i=0; i < vertices.rows(); i++) { 198 | sumWeight = 0.0 ; 199 | // loop over the faces 200 | for (int j=0 ; j < faces.rows() ; j++ ) { 201 | findThisVer = false ; 202 | for (int k = 0 ; k < 3 ; k++) { 203 | if (faces(j,k) == i) { 204 | position = k ; 205 | findThisVer = true ; 206 | break ; 207 | } 208 | } 209 | 210 | if (findThisVer) { 211 | // vertices of the first triangle 212 | PCurrent = vertices.row(i) ; 213 | PNeighCurrent = vertices.row(faces(j, (position+2)%3)) ; 214 | PTriangle1 = vertices.row(faces(j, (position+1)%3)); 215 | 216 | // second triangle information 217 | triangle2 = TT(j, (position+2)%3) ; 218 | edge2 = TTi(j,(position+2)%3) ; 219 | PTriangle2 = vertices.row(faces(triangle2, (edge2+2)%3)) ; 220 | 221 | // compute the sum of the cotan 222 | weight = computeCotan(PTriangle2, PNeighCurrent, PCurrent) + computeCotan(PTriangle1, PNeighCurrent, PCurrent) ; 223 | 224 | // create and push the vector with indices and weight for this neighbor 225 | thisNeighborInfo.push_back(double(faces(j,(position +2)%3))) ; 226 | thisNeighborInfo.push_back(weight) ; 227 | neighborInformation.push_back(thisNeighborInfo) ; 228 | 229 | // clear all for the next neighbor 230 | thisNeighborInfo.clear() ; 231 | weight = 0.0 ; 232 | } 233 | } 234 | 235 | valence = neighborInformation.size() ; 236 | 237 | for (int index =0; index < valence ; index++) { 238 | thisNeight = neighborInformation[index][0] ; 239 | thisWeight = neighborInformation[index][1] ; 240 | 241 | // add the weight to the sum 242 | sumWeight += thisWeight ; 243 | 244 | tripletList.push_back(Eigen::Triplet(i, thisNeight,thisWeight)) ; 245 | } 246 | 247 | tripletList.push_back(Eigen::Triplet(i, i,-sumWeight)); 248 | 249 | // clear all for the next vertex 250 | neighborInformation.clear() ; 251 | sumWeight = 0.0 ; 252 | valence = 0 ; 253 | } 254 | C.setFromTriplets(tripletList.begin(), tripletList.end()); 255 | return C ; 256 | } 257 | 258 | // compute the inverse of a sparse Matrix 259 | Eigen::SparseMatrix inverseSparse(Eigen::SparseMatrix M) { 260 | Eigen::SimplicialLDLT> solver; 261 | solver.compute(M); 262 | Eigen::SparseMatrix I(M.rows(),M.cols()); 263 | I.setIdentity(); 264 | Eigen::SparseMatrix M_inv = solver.solve(I); 265 | return M_inv ; 266 | } 267 | 268 | // compute the laplace operator from C and M matrices 269 | Eigen::SparseMatrix computeCotanDiscretization(DecoratedCloud & cloud) { 270 | int numbVertices = cloud.getVertices().rows() ; 271 | Eigen::SparseMatrix L(numbVertices,numbVertices); 272 | Eigen::SparseMatrix C(numbVertices,numbVertices); 273 | Eigen::SparseMatrix M(numbVertices,numbVertices); 274 | Eigen::SparseMatrix M_inv(numbVertices,numbVertices); 275 | 276 | C = weightCotan(cloud) ; 277 | M = diagonalArea(cloud) ; 278 | M_inv = inverseSparse(M) ; 279 | 280 | L = M_inv*C ; 281 | return L ; 282 | } 283 | 284 | 285 | // ******************* Mean curvature function *************** 286 | Eigen::MatrixXd meanCurvature(DecoratedCloud & cloud, int typeDiscretization) { 287 | // initialization for vertices and normals 288 | int numbVertices = cloud.getVertices().rows() ; 289 | Eigen::MatrixXd normals(numbVertices, 3) ; 290 | Eigen::MatrixXd vertices(numbVertices, 3) ; 291 | normals = cloud.getNormals() ; 292 | vertices = cloud.getVertices() ; 293 | 294 | // compute the L matrix in function of the discretization asked 295 | // can extend this if we compute new types of discretization 296 | Eigen::SparseMatrix L(numbVertices,numbVertices); 297 | if (typeDiscretization == 0) L = uniformLaplacian(cloud) ; 298 | else if (typeDiscretization == 1) L = computeCotanDiscretization(cloud) ; 299 | 300 | // compute the laplacian vectors 301 | Eigen::MatrixXd laplacian_vectors(numbVertices,3) ; 302 | laplacian_vectors = L*vertices ; 303 | 304 | // Compute the magnitude of the mean curvature 305 | Eigen::MatrixXd meancurv(numbVertices,1) ; 306 | meancurv = laplacian_vectors.rowwise().norm()/2.0 ; 307 | 308 | // look for its orientation 309 | Eigen::MatrixXd signCurv(numbVertices,1) ; 310 | // compute the dot product 311 | signCurv = (laplacian_vectors.cwiseProduct(normals)).rowwise().sum(); 312 | // only interested on the sign 313 | signCurv = signCurv.array() / signCurv.array().abs().array() ; 314 | 315 | // invert the curvature if needed 316 | meancurv = meancurv.cwiseProduct(-1.f*signCurv) ; 317 | 318 | // apply this mean curvature as a color for the resulting mesh 319 | Eigen::MatrixXd color(numbVertices,3) ; 320 | igl::jet(meancurv,true,color); 321 | 322 | return color ; 323 | } 324 | 325 | // ******************* Gaussian curvature function *************** 326 | Eigen::MatrixXd gaussianCurvUnifom(DecoratedCloud & cloud) { 327 | // initialization of matrices and variable 328 | int numbVertices = cloud.getVertices().rows() ; 329 | Eigen::MatrixXd gaussianCurv(numbVertices,1) ; 330 | Eigen::MatrixXd vertices(numbVertices, 3) ; 331 | Eigen::MatrixXi faces(numbVertices, 3) ; 332 | vertices = cloud.getVertices() ; 333 | faces = cloud.getFaces() ; 334 | bool findThisVer ; 335 | int position ; 336 | double thisArea, thisAngle, thisGausCurv, sumAngle, areaTotal ; 337 | Eigen::Vector3d P1, P2, P3 ; 338 | 339 | // loop over the vertices 340 | for (int i=0; i < vertices.rows(); i++) { 341 | sumAngle = 0.0 ; 342 | areaTotal = 0.0 ; 343 | thisGausCurv = 0.0 ; 344 | // loop over the faces 345 | for (int j=0 ; j < faces.rows() ; j++ ) { 346 | findThisVer = false ; 347 | for (int k = 0 ; k < 3 ; k++) { 348 | if (faces(j,k) == i) { 349 | position = k ; 350 | findThisVer = true ; 351 | break ; 352 | } 353 | } 354 | 355 | // if the vertex is in the faces, compute the area of the triangle 356 | if (findThisVer) { 357 | // the vertices of this triangle 358 | P1 = vertices.row(i) ; 359 | P2 = vertices.row(faces(j,(position +1)%3)) ; 360 | P3 = vertices.row(faces(j,(position +2)%3)) ; 361 | 362 | // compute the angle for the triangle and the area 363 | thisArea = computeArea(P1, P2, P3) ; 364 | thisAngle = computeAngle(P1, P2, P3) ; 365 | 366 | areaTotal += thisArea ; 367 | sumAngle += thisAngle ; 368 | } 369 | } 370 | // compute the gauss curvature 371 | thisGausCurv = (2.0*Pi-sumAngle)/(areaTotal/3.0) ; 372 | gaussianCurv(i,0) = thisGausCurv ; 373 | } 374 | // apply this gaussian curvature as a color for the resulting mesh 375 | Eigen::MatrixXd color(numbVertices,3) ; 376 | igl::jet(gaussianCurv,true ,color); 377 | 378 | return color ; 379 | } 380 | } // namespace acq -------------------------------------------------------------------------------- /Smoothing/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/normalEstimation.h" 2 | #include "acq/cloudManager.h" 3 | #include "acq/decoratedCloud.h" 4 | #include "acq/discreteCurvature.h" 5 | #include "acq/smoothing.h" 6 | 7 | #include "nanogui/formhelper.h" 8 | #include "nanogui/screen.h" 9 | 10 | #include "igl/readOFF.h" 11 | #include "igl/viewer/Viewer.h" 12 | 13 | #include 14 | 15 | namespace acq { 16 | 17 | /** \brief Re-estimate normals of cloud \p V fitting planes 18 | * to the \p kNeighbours nearest neighbours of each point. 19 | * \param[in ] kNeighbours How many neighbours to use (Typiclaly: 5..15) 20 | * \param[in ] vertices Input pointcloud. Nx3, where N is the number of points. 21 | * \param[in ] maxNeighbourDist Maximum distance between vertex and neighbour. 22 | * \param[out] viewer The viewer to show the normals at. 23 | * \return The estimated normals, Nx3. 24 | */ 25 | NormalsT 26 | recalcNormals( 27 | int const kNeighbours, 28 | CloudT const& vertices, 29 | float const maxNeighbourDist 30 | ) { 31 | NeighboursT const neighbours = 32 | calculateCloudNeighbours( 33 | /* [in] cloud: */ vertices, 34 | /* [in] k-neighbours: */ kNeighbours, 35 | /* [in] maxDist: */ maxNeighbourDist 36 | ); 37 | 38 | // Estimate normals for points in cloud vertices 39 | NormalsT normals = 40 | calculateCloudNormals( 41 | /* [in] Cloud: */ vertices, 42 | /* [in] Lists of neighbours: */ neighbours 43 | ); 44 | 45 | return normals; 46 | } //...recalcNormals() 47 | 48 | void setViewerNormals( 49 | igl::viewer::Viewer & viewer, 50 | CloudT const& vertices, 51 | NormalsT const& normals 52 | ) { 53 | // [Optional] Set viewer face normals for shading 54 | //viewer.data.set_normals(normals); 55 | 56 | // Clear visualized lines (see Viewer.clear()) 57 | viewer.data.lines = Eigen::MatrixXd(0, 9); 58 | 59 | // Add normals to viewer 60 | viewer.data.add_edges( 61 | /* [in] Edge starting points: */ vertices, 62 | /* [in] Edge endpoints: */ vertices + normals * 0.01, // scale normals to 1% length 63 | /* [in] Colors: */ Eigen::Vector3d::Zero() 64 | ); 65 | } 66 | 67 | } //...ns acq 68 | 69 | int main(int argc, char *argv[]) { 70 | 71 | // How many neighbours to use for normal estimation, shown on GUI. 72 | int kNeighbours = 10; 73 | // Maximum distance between vertices to be considered neighbours (FLANN mode) 74 | float maxNeighbourDist = 0.15; //TODO: set to average vertex distance upon read 75 | double pourcentage = 0.0 ; 76 | double lambda = 0.0 ; 77 | bool typeDiscretization = false ; 78 | float noise ; 79 | // Dummy enum to demo GUI 80 | enum MeshType { Bumpty=0, Bunny, Fandisk, Cow, Dragon} typeMesh = Bumpty ; 81 | 82 | // Dummy variable to demo GUI 83 | bool boolVariable = true; 84 | // Dummy variable to demo GUI 85 | float floatVariable = 0.1f; 86 | 87 | // Load a mesh in OFF format 88 | std::string meshPath = "../3rdparty/libigl/tutorial/shared/screwdriver.off"; 89 | if (argc > 1) { 90 | meshPath = std::string(argv[1]); 91 | if (meshPath.find(".off") == std::string::npos) { 92 | std::cerr << "Only ready for OFF files for now...\n"; 93 | return EXIT_FAILURE; 94 | } 95 | } else { 96 | std::cout << "Usage: iglFrameWork ." << "\n"; 97 | } 98 | 99 | std::string meshPath2 = "../bunny.off"; 100 | if (argc > 1) { 101 | meshPath2 = std::string(argv[1]); 102 | if (meshPath2.find(".off") == std::string::npos) { 103 | std::cerr << "Only ready for OFF files for now...\n"; 104 | return EXIT_FAILURE; 105 | } 106 | } else { 107 | std::cout << "Usage: iglFrameWork ." << "\n"; 108 | } 109 | 110 | std::string meshPath3 = "../bumpy.off"; 111 | if (argc > 1) { 112 | meshPath3 = std::string(argv[1]); 113 | if (meshPath3.find(".off") == std::string::npos) { 114 | std::cerr << "Only ready for OFF files for now...\n"; 115 | return EXIT_FAILURE; 116 | } 117 | } else { 118 | std::cout << "Usage: iglFrameWork ." << "\n"; 119 | } 120 | 121 | std::string meshPath4 = "../3rdparty/libigl/tutorial/shared/cow.off"; 122 | if (argc > 1) { 123 | meshPath3 = std::string(argv[1]); 124 | if (meshPath4.find(".off") == std::string::npos) { 125 | std::cerr << "Only ready for OFF files for now...\n"; 126 | return EXIT_FAILURE; 127 | } 128 | } else { 129 | std::cout << "Usage: iglFrameWork ." << "\n"; 130 | } 131 | 132 | std::string meshPath5 = "../dragon.off"; 133 | if (argc > 1) { 134 | meshPath3 = std::string(argv[1]); 135 | if (meshPath5.find(".off") == std::string::npos) { 136 | std::cerr << "Only ready for OFF files for now...\n"; 137 | return EXIT_FAILURE; 138 | } 139 | } else { 140 | std::cout << "Usage: iglFrameWork ." << "\n"; 141 | } 142 | 143 | // Visualize the mesh in a viewer 144 | igl::viewer::Viewer viewer; 145 | { 146 | // Don't show face edges 147 | viewer.core.show_lines = false; 148 | viewer.core.show_overlay = false; 149 | viewer.core.invert_normals = false; 150 | } 151 | 152 | // Store cloud so we can store normals later 153 | acq::CloudManager cloudManager; 154 | // Read mesh from meshPath 155 | { 156 | // Pointcloud vertices, N rows x 3 columns. 157 | Eigen::MatrixXd V; 158 | // Face indices, M x 3 integers referring to V. 159 | Eigen::MatrixXi F; 160 | // Read mesh 161 | igl::readOFF(meshPath, V, F); 162 | // Check, if any vertices read 163 | if (V.rows() <= 0) { 164 | std::cerr << "Could not read mesh at " << meshPath 165 | << "...exiting...\n"; 166 | return EXIT_FAILURE; 167 | } //...if vertices read 168 | 169 | // Pointcloud vertices, N rows x 3 columns. 170 | Eigen::MatrixXd V2; 171 | // Face indices, M x 3 integers referring to V. 172 | Eigen::MatrixXi F2; 173 | // Read mesh 174 | igl::readOFF(meshPath2, V2, F2); 175 | // Check, if any vertices read 176 | if (V2.rows() <= 0) { 177 | std::cerr << "Could not read mesh at " << meshPath2 178 | << "...exiting...\n"; 179 | return EXIT_FAILURE; 180 | } //...if vertices read 181 | 182 | // Pointcloud vertices, N rows x 3 columns. 183 | Eigen::MatrixXd V3; 184 | // Face indices, M x 3 integers referring to V. 185 | Eigen::MatrixXi F3; 186 | // Read mesh 187 | igl::readOFF(meshPath3, V3, F3); 188 | // Check, if any vertices read 189 | if (V3.rows() <= 0) { 190 | std::cerr << "Could not read mesh at " << meshPath3 191 | << "...exiting...\n"; 192 | return EXIT_FAILURE; 193 | } //...if vertices read 194 | 195 | // Pointcloud vertices, N rows x 3 columns. 196 | Eigen::MatrixXd V4; 197 | // Face indices, M x 3 integers referring to V. 198 | Eigen::MatrixXi F4; 199 | // Read mesh 200 | igl::readOFF(meshPath4, V4, F4); 201 | // Check, if any vertices read 202 | if (V4.rows() <= 0) { 203 | std::cerr << "Could not read mesh at " << meshPath3 204 | << "...exiting...\n"; 205 | return EXIT_FAILURE; 206 | } //...if vertices read 207 | 208 | Eigen::MatrixXd V5; 209 | // Face indices, M x 3 integers referring to V. 210 | Eigen::MatrixXi F5; 211 | // Read mesh 212 | igl::readOFF(meshPath5, V5, F5); 213 | // Check, if any vertices read 214 | if (V5.rows() <= 0) { 215 | std::cerr << "Could not read mesh at " << meshPath5 216 | << "...exiting...\n"; 217 | return EXIT_FAILURE; 218 | } //...if vertices read 219 | 220 | for (int i=0; i<2; i++) { 221 | cloudManager.addCloud(acq::DecoratedCloud(V3, F3)); 222 | cloudManager.addCloud(acq::DecoratedCloud(V2, F2)); 223 | cloudManager.addCloud(acq::DecoratedCloud(V, F)); 224 | cloudManager.addCloud(acq::DecoratedCloud(V4, F4)); 225 | cloudManager.addCloud(acq::DecoratedCloud(V5, F5)); 226 | } 227 | // Show mesh 228 | viewer.data.set_mesh( 229 | cloudManager.getCloud(typeMesh).getVertices(), 230 | cloudManager.getCloud(typeMesh).getFaces() 231 | ); 232 | 233 | 234 | } //...read mesh 235 | 236 | // Extend viewer menu using a lambda function 237 | viewer.callback_init = 238 | [ 239 | &cloudManager, &kNeighbours, &maxNeighbourDist, &noise, 240 | &floatVariable, &boolVariable, &typeMesh, &lambda, &pourcentage, &typeDiscretization 241 | ] (igl::viewer::Viewer& viewer) 242 | { 243 | // Add an additional menu window 244 | viewer.ngui->addWindow(Eigen::Vector2i(900,10), "Acquisition3D"); 245 | 246 | viewer.ngui->addGroup("Choose your mesh"); 247 | 248 | viewer.ngui->addVariable("Which mesh do you want ?",typeMesh)->setItems( 249 | {"Bumpty","Bunny","Screwdriver", "Cow", "Dragon"} 250 | ); 251 | 252 | viewer.ngui->addButton("Show new mesh", 253 | [&]() { 254 | viewer.data.clear() ; 255 | 256 | // Show mesh 257 | viewer.data.set_mesh( 258 | cloudManager.getCloud(typeMesh).getVertices(), 259 | cloudManager.getCloud(typeMesh).getFaces() 260 | ); 261 | }); 262 | 263 | viewer.ngui->addButton("Back to the original mesh", 264 | [&]() { 265 | cloudManager.setCloud(cloudManager.getCloud(typeMesh+5), typeMesh) ; 266 | viewer.data.clear() ; 267 | 268 | // Show mesh 269 | viewer.data.set_mesh( 270 | cloudManager.getCloud(typeMesh).getVertices(), 271 | cloudManager.getCloud(typeMesh).getFaces() 272 | ); 273 | }); 274 | 275 | viewer.ngui->addGroup("Set normals for the mesh"); 276 | 277 | viewer.ngui->addButton("Estimate normals (from faces)",[&]() { 278 | 279 | if (!cloudManager.getCloud(typeMesh).hasNormals()) 280 | cloudManager.getCloud(typeMesh).setNormals( 281 | acq::recalcNormals( 282 | /* [in] K-neighbours for FLANN: */ kNeighbours, 283 | /* [in] Vertices matrix: */ cloudManager.getCloud(typeMesh).getVertices(), 284 | /* [in] max neighbour distance: */ maxNeighbourDist 285 | ) 286 | ); 287 | 288 | // Estimate neighbours using FLANN 289 | acq::NeighboursT const neighbours = 290 | acq::calculateCloudNeighboursFromFaces( 291 | /* [in] Faces: */ cloudManager.getCloud(typeMesh).getFaces() 292 | ); 293 | 294 | // Estimate normals for points in cloud vertices 295 | cloudManager.getCloud(typeMesh).setNormals( 296 | acq::calculateCloudNormals( 297 | /* [in] Cloud: */ cloudManager.getCloud(typeMesh).getVertices(), 298 | /* [in] Lists of neighbours: */ neighbours 299 | ) 300 | ); 301 | 302 | // Update viewer 303 | acq::setViewerNormals( 304 | /* [in, out] Viewer to update: */ viewer, 305 | /* [in] Pointcloud: */ cloudManager.getCloud(typeMesh).getVertices(), 306 | /* [in] Normals of Pointcloud: */ cloudManager.getCloud(typeMesh).getNormals() 307 | ); 308 | } //...button push lambda 309 | ); //...estimate normals from faces 310 | 311 | // Add a button for orienting normals using face information 312 | viewer.ngui->addButton( 313 | /* Displayed label: */ "Orient normals (from faces)", 314 | 315 | /* Lambda to call: */ [&]() { 316 | // Check, if normals already exist 317 | if (!cloudManager.getCloud(typeMesh).hasNormals()) 318 | cloudManager.getCloud(typeMesh).setNormals( 319 | acq::recalcNormals( 320 | /* [in] K-neighbours for FLANN: */ kNeighbours, 321 | /* [in] Vertices matrix: */ cloudManager.getCloud(typeMesh).getVertices(), 322 | /* [in] max neighbour distance: */ maxNeighbourDist 323 | ) 324 | ); 325 | 326 | // Orient normals in place using established neighbourhood 327 | int nFlips = 328 | acq::orientCloudNormalsFromFaces( 329 | /* [in ] Lists of neighbours: */ cloudManager.getCloud(typeMesh).getFaces(), 330 | /* [in,out] Normals to change: */ cloudManager.getCloud(typeMesh).getNormals() 331 | ); 332 | 333 | // Update viewer 334 | acq::setViewerNormals( 335 | /* [in, out] Viewer to update: */ viewer, 336 | /* [in] Pointcloud: */ cloudManager.getCloud(typeMesh).getVertices(), 337 | /* [in] Normals of Pointcloud: */ cloudManager.getCloud(typeMesh).getNormals() 338 | ); 339 | } 340 | ); 341 | 342 | 343 | // Add new group 344 | viewer.ngui->addGroup("Mean curvature"); 345 | 346 | viewer.ngui->addButton("Using uniform discretization", 347 | [&]() { 348 | // apply the mean curvature uniform 349 | acq::DecoratedCloud &cloud = cloudManager.getCloud(typeMesh); 350 | Eigen::MatrixXd color ; 351 | color = meanCurvature(cloud, 0) ; 352 | 353 | // change the color of the mesh 354 | viewer.data.set_colors(color); 355 | 356 | }); 357 | 358 | viewer.ngui->addButton("Using cotan discretization", 359 | [&]() { 360 | acq::DecoratedCloud &cloud = cloudManager.getCloud(typeMesh); 361 | Eigen::MatrixXd color ; 362 | color = meanCurvature(cloud, 1) ; 363 | 364 | viewer.data.set_colors(color); 365 | }); 366 | 367 | 368 | viewer.ngui->addGroup("Gaussian curvature"); 369 | 370 | viewer.ngui->addButton("Discrete Gaussian curvature", 371 | [&]() { 372 | acq::DecoratedCloud &cloud = cloudManager.getCloud(typeMesh); 373 | Eigen::MatrixXd color ; 374 | color = gaussianCurvUnifom(cloud) ; 375 | viewer.data.set_colors(color); 376 | }); 377 | 378 | viewer.ngui->addGroup("Smoothing"); 379 | 380 | // ask for the poucentage of the bounding box instead of lambda 381 | viewer.ngui->addVariable("% of the diagonal BB (int) : ",[&] (double val) { 382 | pourcentage = val; 383 | lambda = findLambda(pourcentage,cloudManager.getCloud(typeMesh)) ; }, 384 | [&]() { 385 | return lambda; 386 | } ); 387 | 388 | 389 | viewer.ngui->addVariable( 390 | "Cotan distretization : ", 391 | [&](bool val) { 392 | typeDiscretization = val; // set 393 | }, 394 | [&]() { 395 | return typeDiscretization; // get 396 | }); 397 | 398 | 399 | viewer.ngui->addButton("Explicit smoothing", 400 | [&]() { 401 | 402 | // set the vertices 403 | cloudManager.getCloud(typeMesh).setVertices(explicitSmoothing(cloudManager.getCloud(typeMesh), lambda,typeDiscretization)) ; 404 | 405 | // set the normals 406 | acq::NeighboursT const neighbours = 407 | acq::calculateCloudNeighboursFromFaces( 408 | cloudManager.getCloud(typeMesh).getFaces() 409 | ); 410 | 411 | // Estimate normals for points in cloud vertices 412 | cloudManager.getCloud(typeMesh).setNormals( 413 | acq::calculateCloudNormals( 414 | cloudManager.getCloud(typeMesh).getVertices(), 415 | neighbours 416 | ) 417 | ); 418 | 419 | int nFlips = 420 | acq::orientCloudNormalsFromFaces( 421 | cloudManager.getCloud(typeMesh).getFaces(), 422 | cloudManager.getCloud(typeMesh).getNormals() 423 | ); 424 | 425 | viewer.data.clear() ; 426 | 427 | // Show mesh 428 | viewer.data.set_mesh( 429 | cloudManager.getCloud(typeMesh).getVertices(), 430 | cloudManager.getCloud(typeMesh).getFaces() 431 | ); 432 | 433 | Eigen::MatrixXd color ; 434 | color = meanCurvature(cloudManager.getCloud(typeMesh), 1) ; 435 | 436 | viewer.data.set_colors(color) ; 437 | 438 | }); 439 | 440 | viewer.ngui->addButton("Implicit smoothing", 441 | [&]() { 442 | // set the vertices 443 | cloudManager.getCloud(typeMesh).setVertices(implicitSmoothing(cloudManager.getCloud(typeMesh), lambda)) ; 444 | 445 | // set the normals 446 | acq::NeighboursT const neighbours = 447 | acq::calculateCloudNeighboursFromFaces( 448 | cloudManager.getCloud(typeMesh).getFaces() 449 | ); 450 | 451 | 452 | cloudManager.getCloud(typeMesh).setNormals( 453 | acq::calculateCloudNormals( 454 | cloudManager.getCloud(typeMesh).getVertices(), 455 | neighbours 456 | ) 457 | ); 458 | 459 | int nFlips = 460 | acq::orientCloudNormalsFromFaces( 461 | cloudManager.getCloud(typeMesh).getFaces(), 462 | cloudManager.getCloud(typeMesh).getNormals() 463 | ); 464 | 465 | viewer.data.clear() ; 466 | 467 | // Show mesh 468 | viewer.data.set_mesh( 469 | cloudManager.getCloud(typeMesh).getVertices(), 470 | cloudManager.getCloud(typeMesh).getFaces() 471 | ); 472 | 473 | Eigen::MatrixXd color ; 474 | color = meanCurvature(cloudManager.getCloud(typeMesh), 1) ; 475 | viewer.data.set_colors(color); 476 | 477 | }); 478 | 479 | viewer.ngui->addGroup("Test denoising"); 480 | 481 | 482 | viewer.ngui->addVariable( "% of noise", [&] (float val) { 483 | noise = val;}, [&]() { 484 | return noise; // get 485 | } ); 486 | 487 | viewer.ngui->addButton("Add noise",[&]() { 488 | cloudManager.getCloud(typeMesh).setVertices(addNoise(noise, cloudManager.getCloud(typeMesh))) ; 489 | 490 | cloudManager.getCloud(typeMesh).setNormals( 491 | acq::recalcNormals( 492 | kNeighbours, 493 | cloudManager.getCloud(typeMesh).getVertices(), 494 | maxNeighbourDist 495 | ) 496 | ); 497 | // Show mesh 498 | viewer.data.clear() ; 499 | viewer.data.set_mesh( 500 | cloudManager.getCloud(typeMesh).getVertices(), 501 | cloudManager.getCloud(typeMesh).getFaces() 502 | ); 503 | viewer.data.set_colors(cloudManager.getCloud(typeMesh).getColors()); 504 | }); 505 | 506 | 507 | viewer.ngui->addButton("Compute the error",[&]() { 508 | float error ; 509 | error = computeError(cloudManager.getCloud(typeMesh),cloudManager.getCloud(typeMesh+5)) ; 510 | std::cout << "The error is : " << error << std::endl ; 511 | }); 512 | 513 | // Generate menu 514 | viewer.screen->performLayout(); 515 | return false; 516 | }; //...viewer menu 517 | 518 | // Start viewer 519 | viewer.launch(); 520 | return 0; 521 | } //...main() 522 | -------------------------------------------------------------------------------- /Smoothing/src/normalEstimation.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Aron Monszpart on 19/01/17. 3 | // 4 | 5 | #include "acq/normalEstimation.h" 6 | 7 | #include "acq/impl/normalEstimation.hpp" // Templated functions 8 | 9 | #include "nanoflann/nanoflann.hpp" // Nearest neighbour lookup in a pointcloud 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace acq { 16 | 17 | NeighboursT 18 | calculateCloudNeighbours( 19 | CloudT const& cloud, 20 | int const k, 21 | float const maxDist, 22 | int const maxLeafs 23 | ) { 24 | // Floating point type 25 | typedef typename CloudT::Scalar Scalar; 26 | // Point dimensions 27 | enum { Dim = 3 }; 28 | // Copy-free Eigen->FLANN wrapper 29 | typedef nanoflann::KDTreeEigenMatrixAdaptor < 30 | /* Eigen matrix type: */ CloudT, 31 | /* Space dimensionality: */ Dim, 32 | /* Distance metric: */ nanoflann::metric_L2 33 | > KdTreeWrapperT; 34 | 35 | // Squared max distance 36 | float const maxDistSqr = maxDist * maxDist; 37 | 38 | // Safety check dimensionality 39 | if (cloud.cols() != Dim) { 40 | std::cerr << "Point dimension mismatch: " << cloud.cols() 41 | << " vs. " << Dim 42 | << "\n"; 43 | throw new std::runtime_error("Point dimension mismatch"); 44 | } //...check dimensionality 45 | 46 | // Build KdTree 47 | KdTreeWrapperT cloudIndex(Dim, cloud, maxLeafs); 48 | cloudIndex.index->buildIndex(); 49 | 50 | // Neighbour indices 51 | std::vector neighbourIndices(k); 52 | std::vector distsSqr(k); 53 | 54 | // Placeholder structure for nanoFLANN 55 | nanoflann::KNNResultSet resultSet(k); 56 | 57 | // Associative list of neighbours: { pointId => [neighbourId_0, nId_1, ... nId_k-1] } 58 | NeighboursT neighbours; 59 | // For each point, store normal 60 | for (int pointId = 0; pointId != cloud.rows(); ++pointId) { 61 | // Initialize nearest neighhbour estimation 62 | resultSet.init(&neighbourIndices[0], &distsSqr[0]); 63 | 64 | // Make sure it's ok to expose raw data pointer of point 65 | static_assert( 66 | std::is_same::value, 67 | "Double input assumed next. Otherwise, explicit copy is needed!" 68 | ); 69 | 70 | // Find neighbours of point in "pointId"-th row 71 | cloudIndex.index->findNeighbors( 72 | /* Output wrapper: */ resultSet, 73 | /* Query point double[3] pointer: */ cloud.row(pointId).data(), 74 | /* How many neighbours to use: */ nanoflann::SearchParams(k) 75 | ); 76 | 77 | // Filter neighbours by squared distance 78 | NeighboursT::mapped_type currNeighbours; 79 | for (int i = 0; i != neighbourIndices.size(); ++i) { 80 | // if not same point and close enough 81 | if ((neighbourIndices[i] != pointId ) && 82 | (distsSqr [i] < maxDistSqr)) 83 | currNeighbours.insert(neighbourIndices[i]); 84 | } 85 | 86 | // Store list of neighbours 87 | std::pair const success = 88 | neighbours.insert( 89 | std::make_pair( 90 | pointId, 91 | currNeighbours 92 | ) 93 | ); 94 | 95 | if (!success.second) 96 | std::cerr << "Could not store neighbours of point " << pointId 97 | << ", already inserted?\n"; 98 | 99 | } //...for all points 100 | 101 | // return estimated normals 102 | return neighbours; 103 | } //...calculateCloudNormals() 104 | 105 | NormalsT 106 | calculateCloudNormals( 107 | CloudT const& cloud, 108 | NeighboursT const& neighbours 109 | ) { 110 | // Output normals: N x 3 111 | CloudT normals(cloud.rows(), 3); 112 | 113 | // For each point, store normal 114 | for (int pointId = 0; pointId != cloud.rows(); ++pointId) { 115 | // Estimate vertex normal from neighbourhood indices and cloud 116 | normals.row(pointId) = 117 | calculatePointNormal( 118 | /* PointCloud: */ cloud, 119 | /* ID of vertex: */ pointId, 120 | /* Ids of neighbours: */ neighbours.at(pointId) 121 | ); 122 | } //...for all points 123 | 124 | // Return estimated normals 125 | return normals; 126 | } //...calculateCloudNormals() 127 | 128 | int 129 | orientCloudNormals( 130 | NeighboursT const& neighbours, 131 | NormalsT & normals 132 | ) { 133 | if (!normals.size()) { 134 | std::cerr << "[orientCloudNormals] No normals to work on...\n"; 135 | return -1; 136 | } 137 | 138 | // List of points to visit 139 | std::queue queue; 140 | // Quick-lookup unique container of already visited points 141 | std::set visited; 142 | 143 | // Count changes 144 | int nFlips = 0; 145 | 146 | while (visited.size() != normals.size()) { 147 | // Traverse a connected component 148 | if (queue.empty()) { 149 | if (!visited.size()) { 150 | // Initialize queue with one random point 151 | queue.push(rand() % normals.size()); // TODO: pick point with low curvature 152 | } else { 153 | // Expand queue with first unvisited point 154 | for (int i = 0; i != normals.size() && queue.empty(); ++i) { 155 | // if unvisited, use 156 | if (visited.find(i) == visited.end()) 157 | queue.push(i); // enqueue 158 | } //...for each point 159 | } //...next component 160 | 161 | // Set visited 162 | visited.insert(queue.front()); 163 | } //...if queue empty 164 | 165 | // While points to visit exist 166 | while (!queue.empty()) { 167 | // Read next point from queue 168 | int const pointId = queue.front(); 169 | // Remove point from queue 170 | queue.pop(); 171 | 172 | // Fetch neighbours 173 | NeighboursT::const_iterator const iter = neighbours.find(pointId); 174 | // Check, if any neighbours 175 | if (iter == neighbours.end()) { 176 | //std::cerr << "Could not find neighbours of point " << pointId << "\n"; 177 | continue; 178 | } 179 | 180 | for (int const neighbourId : iter->second) { 181 | // If unvisited 182 | if (visited.find(neighbourId) == visited.end()) { 183 | // Enqueue for next level 184 | queue.push(neighbourId); 185 | // Mark visited 186 | visited.insert(neighbourId); 187 | 188 | // Flip neighbour normal, if not same direction as precursor point 189 | if (normals.row(pointId).dot(normals.row(neighbourId)) < 0.f) { 190 | normals.row(neighbourId) *= -1.f; 191 | ++nFlips; 192 | } 193 | } //...if neighbour unvisited 194 | } //...for each neighbour of point 195 | } //...while points in queue 196 | } 197 | 198 | return nFlips; 199 | } //...orientCloudNormals() 200 | 201 | } //...ns acq 202 | 203 | 204 | // 205 | // Template instantiation 206 | // 207 | 208 | namespace acq { 209 | 210 | template int 211 | orientCloudNormalsFromFaces( 212 | FacesT const& faces, 213 | NormalsT & normals 214 | ); 215 | 216 | template NeighboursT 217 | calculateCloudNeighboursFromFaces( 218 | FacesT const& faces 219 | ); 220 | 221 | } //...ns acq 222 | -------------------------------------------------------------------------------- /Smoothing/src/smoothing.cpp: -------------------------------------------------------------------------------- 1 | #include "acq/smoothing.h" 2 | 3 | namespace acq { 4 | 5 | double findLambda(double pourcentage, DecoratedCloud &cloud) { 6 | double sizeBBDiag = igl::bounding_box_diagonal(cloud.getVertices()) ; 7 | double result = (pourcentage/100.0)*sizeBBDiag ; 8 | 9 | return result ; 10 | } 11 | 12 | // ***********************$ Function to add noise and test smoothing ****************** 13 | void computeBoundingBox(float &Xmax,float & Xmin,float & Ymax,float & Ymin,float &Zmax, float & Zmin, DecoratedCloud& cloud) { 14 | Eigen::MatrixXd maximum(1, 3) ; 15 | maximum = cloud.getVertices().colwise().maxCoeff() ; 16 | Xmax = maximum(0) ; 17 | Ymax= maximum(1) ; 18 | Zmax = maximum(2) ; 19 | 20 | Eigen::MatrixXd minimum(1, 3) ; 21 | minimum = cloud.getVertices().colwise().minCoeff() ; 22 | Xmin = minimum(0) ; 23 | Ymin = minimum(1) ; 24 | Zmin = minimum(2) ; 25 | } 26 | 27 | Eigen::MatrixXd addNoise(float noise, DecoratedCloud& cloud) { 28 | // compute the value of the boundingBox for the second cloud 29 | float Xmax, Xmin, Ymax, Ymin, Zmax, Zmin ; 30 | computeBoundingBox(Xmax, Xmin, Ymax, Ymin,Zmax, Zmin, cloud) ; 31 | 32 | // set the variance to sigma = noise% of the bouding box size in each direction 33 | float sigmaX =(Xmax-Xmin)*noise ; 34 | float sigmaY = (Ymax-Ymin)*noise ; 35 | float sigmaZ = (Zmax-Zmin)*noise ; 36 | 37 | int M = cloud.getVertices().rows() ; 38 | 39 | // initialization 40 | Eigen::MatrixXd random(M, 3) ; 41 | 42 | // construct noise 43 | for(int i = 0; i< M; i++) { 44 | random(i, 0) = std::rand()*sigmaX /RAND_MAX ; 45 | random(i, 1) = std::rand()*sigmaY /RAND_MAX ; 46 | random(i, 2) = std::rand()*sigmaZ /RAND_MAX ; 47 | } 48 | 49 | Eigen::MatrixXd newVertices(M,3) ; 50 | // add noise to the vertices 51 | newVertices = cloud.getVertices() + random ; 52 | 53 | return newVertices ; 54 | } 55 | 56 | // *********************** Smoothing explicit ****************** 57 | Eigen::MatrixXd explicitSmoothing(DecoratedCloud& cloud, double lambda, bool typeDiscretization) { 58 | int numbVertices = cloud.getVertices().rows() ; 59 | 60 | //****** Create the L matrix ******* 61 | Eigen::SparseMatrix L(numbVertices,numbVertices); 62 | 63 | if (typeDiscretization) L = computeCotanDiscretization(cloud) ; 64 | else L = uniformLaplacian(cloud) ; 65 | 66 | // ***** Move the vertices ****** 67 | Eigen::MatrixXd vertices_new(numbVertices,3) ; 68 | Eigen::SparseMatrix eye(numbVertices,numbVertices) ; 69 | eye.setIdentity() ; 70 | 71 | vertices_new = (eye + L*lambda)*cloud.getVertices() ; 72 | 73 | return vertices_new ; 74 | } 75 | 76 | // *********************** Smoothing implicit ****************** 77 | Eigen::MatrixXd implicitSmoothing(DecoratedCloud& cloud, double lambda) { 78 | int numbVertices = cloud.getVertices().rows() ; 79 | 80 | //****** Create the Lw matrix ******* 81 | Eigen::SparseMatrix Lw(numbVertices,numbVertices); 82 | Lw = weightCotan(cloud) ; 83 | 84 | //****** Create the M matrix ******* 85 | Eigen::SparseMatrix M(numbVertices,numbVertices); 86 | M = diagonalArea(cloud) ; 87 | 88 | // ***** Create the equation Ax = b ****** 89 | Eigen::SparseMatrix A(numbVertices,numbVertices); 90 | A = M - lambda*Lw ; 91 | 92 | Eigen::MatrixXd b(numbVertices,3) ; 93 | b = M*cloud.getVertices() ; 94 | 95 | // ***** Compute the solver ****** 96 | Eigen::SimplicialLLT> solver ; 97 | solver.compute(A); 98 | 99 | // **** Solve the system ***** 100 | Eigen::MatrixXd vertices_new(numbVertices,3) ; 101 | vertices_new = solver.solve(b).eval(); 102 | return vertices_new ; 103 | } 104 | 105 | // ********* Function to test denoising ********* 106 | float computeError(DecoratedCloud &cloud1, DecoratedCloud &cloud2) { 107 | float error = (cloud1.getVertices()-cloud2.getVertices()).rowwise().norm().sum() ; 108 | return error ; 109 | } 110 | 111 | 112 | } // namespace acq --------------------------------------------------------------------------------