3D-3D Registration based on PCL

COPYING.md

@@ -8,13 +8,20 @@ AliceVision is licensed under the [MPL2 license](LICENSE-MPL2.md).
 modules are based on the libmv code released under MIT, see [LICENSE-MIT-libmv.md](LICENSE-MIT-libmv.md).
 
 
-It also contains one file with different license:
+Other copyrights and licenses embedded:
 
 *   __kvld__  
     [src/aliceVision/matching/kvld](src/aliceVision/matching/kvld)  
     Copyright (C) 2011-12 Zhe Liu and Pierre Moulon.  
     This file was first released under BSD-2-Clause license, see [LICENSE-BSD-2-Clause](http://opensource.org/licenses/BSD-2-Clause).
 
+*   __sparseICP__
+    [The 3D-3D registration module](src\aliceVision\registration) is based on sparceicp implementation:
+    https://github.com/OpenGP/sparseicp
+    Copyright (C) 2013  LGG, EPFL
+    "Sparse Iterative Closest Point" by Sofien Bouaziz, Andrea Tagliasacchi, Mark Pauly
+    This project was originally released under [MPL2 license](LICENSE-MPL2.md).
+
 
 ## Third parties licenses
 
@@ -141,11 +148,17 @@ This program is based on works distributed under the terms of another license(s)
     and others.  All Rights Reserved.  
     Distributed under the terms of the Eclipse Public License (EPL).
 
+*   __PCL__ (optional)
+    [Point Cloud Library (PCL)](www.pointclouds.org)
+    Copyright (c) 2010-2011, Willow Garage, Inc.
+    Distributed under the [BSD License](http://www.opensource.org/licenses/bsd-license.php).
+
 *   __PopSift__ (optional)  
     [https://github.com/alicevision/popsift](https://github.com/alicevision/popsift)  
     Copyright 2016, Simula Research Laboratory  
     Distributed under the [MPL2 license](http://opensource.org/licenses/MPL-2.0).
     SIFT was patented in the United States from 1999-03-08 to 2020-03-28. See the [patent link](https://patents.google.com/patent/US6711293B1/en) for more information.
+
 *   __vectorGraphics__  
     Copyright (c) Pierre Moulon  
     Distributed under the [MPL2 license](http://opensource.org/licenses/MPL-2.0).

src/CMakeLists.txt

@@ -51,6 +51,7 @@ option(ALICEVISION_BUILD_HDR "Build AliceVision HDR part" ON)
 option(ALICEVISION_BUILD_SOFTWARE "Build AliceVision command line tools." ON)
 option(ALICEVISION_BUILD_EXAMPLES "Build AliceVision samples applications." OFF)
 option(ALICEVISION_BUILD_COVERAGE "Enable code coverage generation (gcc only)" OFF)
+option(ALICEVISION_BUILD_REGISTRATION "Build AliceVision Registration part" OFF)
 trilean_option(ALICEVISION_BUILD_DOC "Build AliceVision documentation" AUTO)
 
 trilean_option(ALICEVISION_USE_OPENMP "Enable OpenMP parallelization" ON)
@@ -413,14 +414,13 @@ endif()
 # - external if FLANN_INCLUDE_DIR_HINTS and a valid Flann setup is found
 # ==============================================================================
 if(ALICEVISION_BUILD_SFM)
-  if(NOT DEFINED FLANN_INCLUDE_DIR_HINTS)
+  find_package(flann QUIET)
+  if(NOT FLANN_FOUND OR ALICEVISION_USE_INTERNAL_FLANN)
     set(FLANN_INCLUDE_DIR_HINTS ${CMAKE_CURRENT_SOURCE_DIR}/dependencies/flann/src/cpp)
     set(ALICEVISION_USE_INTERNAL_FLANN ON)
-  endif()
-  find_package(Flann QUIET)
-  if(NOT FLANN_FOUND OR ALICEVISION_USE_INTERNAL_FLANN)
     set(FLANN_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/dependencies/flann/src/cpp)
-    set(FLANN_LIBRARY flann_cpp_s)
+    set(FLANN_LIBRARY flann::flann_cpp_s)
+    set(FLANN_LIBRARIES ${FLANN_LIBRARY})
   endif()
 endif()
 
@@ -596,6 +596,25 @@ if(ALICEVISION_BUILD_MVS)
   message(STATUS "Geogram: ${GEOGRAM_LIBRARY}, ${GEOGRAM_INCLUDE_DIR}")
 endif()
 
+# ==============================================================================
+# Point Cloud Library (PCL)
+# ==============================================================================
+if(ALICEVISION_BUILD_REGISTRATION)
+  set(AV_Boost_LIBRARIES ${Boost_LIBRARIES})
+  set(AV_FLANN_LIBRARY ${FLANN_LIBRARY})
+  set(AV_FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIRS})
+  find_package(PCL 1.8 REQUIRED COMPONENTS registration io common octree kdtree search sample_consensus features filters)
+
+  message(STATUS "PCL_LIBRARIES: ${PCL_LIBRARIES}")
+  message(STATUS "PCL_INCLUDE_DIRS: ${PCL_INCLUDE_DIRS}")
+
+  include_directories (${PCL_INCLUDE_DIRS})
+  set(Boost_LIBRARIES ${AV_Boost_LIBRARIES})
+  set(BOOST_LIBRARIES ${AV_Boost_LIBRARIES})
+  set(FLANN_LIBRARY   ${AV_FLANN_LIBRARY})
+  set(FLANN_INCLUDE_DIRS ${AV_FLANN_INCLUDE_DIRS})
+endif()
+
 # ==============================================================================
 # MeshSDFilter
 # ==============================================================================

src/aliceVision/CMakeLists.txt

@@ -38,6 +38,10 @@ if(ALICEVISION_BUILD_SFM)
     add_subdirectory(imageMasking)
     add_subdirectory(keyframe)
   endif()
+
+  if(ALICEVISION_BUILD_REGISTRATION)
+    add_subdirectory(registration)
+  endif()
 endif()
 
 if(ALICEVISION_BUILD_HDR)

src/aliceVision/registration/CMakeLists.txt

@@ -0,0 +1,22 @@
+# Headers
+set(registration_files_headers
+  nanoflannKDTreeEigenAdaptor.hpp
+  rigidMotionEstimator.hpp
+  directSimilarityEstimator.hpp
+  ICP.hpp
+  SICP.hpp
+  PointcloudRegistration.hpp
+)
+
+# Sources
+set(registration_files_sources
+  PointcloudRegistration.cpp
+)
+
+alicevision_add_library(aliceVision_registration
+        SOURCES ${registration_files_headers} ${registration_files_sources}
+        PUBLIC_LINKS
+        Boost::filesystem
+        ${PCL_LIBRARIES}
+        nanoflann
+        )

src/aliceVision/registration/ICP.hpp

@@ -0,0 +1,215 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2018 AliceVision contributors.
+// Copyright (C) 2013  LGG, EPFL
+//   "Sparse Iterative Closest Point" by Sofien Bouaziz, Andrea Tagliasacchi, Mark Pauly
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+#pragma once
+
+#include <Eigen/Dense>
+
+#include "nanoflannKDTreeEigenAdaptor.hpp"
+#include "rigidMotionEstimator.hpp"
+#include "directSimilarityEstimator.hpp"
+
+
+namespace aliceVision {
+namespace registration {
+
+/// ICP implementation using iterative reweighting
+namespace ICP {
+
+enum Function {
+    PNORM,
+    TUKEY,
+    FAIR,
+    LOGISTIC,
+    TRIMMED,
+    NONE
+};
+struct Parameters {
+    Function f = NONE;    /// robust function type
+    double p = 1.0; // 0.1;       /// paramter of the robust function
+    int max_icp = 100;    /// max ICP iteration
+    int max_outer = 100;  /// max outer iteration
+    double stop = 1e-5;   /// stopping criteria
+    bool useDirectSimilarity = false; /// use direct similarity instead of rigid motion
+};
+/// Weight functions
+/// @param Residuals
+/// @param Parameter
+void uniform_weight(Eigen::VectorXd& r) {
+    r = Eigen::VectorXd::Ones(r.rows());
+}
+/// @param Residuals
+/// @param Parameter
+void pnorm_weight(Eigen::VectorXd& r, double p, double reg=1e-8) {
+    for(int i=0; i<r.rows(); ++i) {
+        r(i) = p/(std::pow(r(i),2-p) + reg);
+    }
+}
+/// @param Residuals
+/// @param Parameter
+void tukey_weight(Eigen::VectorXd& r, double p) {
+    for(int i=0; i<r.rows(); ++i) {
+        if(r(i) > p) r(i) = 0.0;
+        else r(i) = std::pow((1.0 - std::pow(r(i)/p,2.0)), 2.0);
+    }
+}
+/// @param Residuals
+/// @param Parameter
+void fair_weight(Eigen::VectorXd& r, double p) {
+    for(int i=0; i<r.rows(); ++i) {
+        r(i) = 1.0/(1.0 + r(i)/p);
+    }
+}
+/// @param Residuals
+/// @param Parameter
+void logistic_weight(Eigen::VectorXd& r, double p) {
+    for(int i=0; i<r.rows(); ++i) {
+        r(i) = (p/r(i))*std::tanh(r(i)/p);
+    }
+}
+struct sort_pred {
+    bool operator()(const std::pair<int,double> &left,
+                    const std::pair<int,double> &right) {
+        return left.second < right.second;
+    }
+};
+/// @param Residuals
+/// @param Parameter
+void trimmed_weight(Eigen::VectorXd& r, double p) {
+    std::vector<std::pair<int, double> > sortedDist(r.rows());
+    for(int i=0; i<r.rows(); ++i) {
+        sortedDist[i] = std::pair<int, double>(i,r(i));
+    }
+    std::sort(sortedDist.begin(), sortedDist.end(), sort_pred());
+    r.setZero();
+    int nbV = r.rows()*p;
+    for(int i=0; i<nbV; ++i) {
+        r(sortedDist[i].first) = 1.0;
+    }
+}
+/// @param Function type
+/// @param Residuals
+/// @param Parameter
+void robust_weight(Function f, Eigen::VectorXd& r, double p) {
+    switch(f) {
+        case PNORM: pnorm_weight(r,p); break;
+        case TUKEY: tukey_weight(r,p); break;
+        case FAIR: fair_weight(r,p); break;
+        case LOGISTIC: logistic_weight(r,p); break;
+        case TRIMMED: trimmed_weight(r,p); break;
+        case NONE: uniform_weight(r); break;
+        default: uniform_weight(r); break;
+    }
+}
+/// Reweighted ICP with point to point
+/// @param Source (one 3D point per column)
+/// @param Target (one 3D point per column)
+/// @param Parameters
+Eigen::Affine3d point_to_point(Eigen::Matrix3Xd& X,
+                    Eigen::Matrix3Xd& Y,
+                    Parameters par = Parameters()) {
+    /// Build kd-tree
+    nanoflann::KDTreeAdaptor<Eigen::Matrix3Xd, 3, nanoflann::metric_L2_Simple> kdtree(Y);
+    /// Buffers
+    Eigen::Matrix3Xd Q = Eigen::Matrix3Xd::Zero(3, X.cols());
+    Eigen::VectorXd W = Eigen::VectorXd::Zero(X.cols());
+    Eigen::Matrix3Xd Xo1 = X;
+    Eigen::Matrix3Xd Xo2 = X;
+    Eigen::Affine3d finalTransform = Eigen::Affine3d::Identity();
+    /// ICP
+    for(int icp=0; icp<par.max_icp; ++icp) {
+        /// Find closest point
+        #pragma omp parallel for
+        for(int i=0; i<X.cols(); ++i) {
+            Q.col(i) = Y.col(kdtree.closest(X.col(i).data()));
+        }
+        /// Computer rotation and translation
+        for(int outer=0; outer<par.max_outer; ++outer) {
+            /// Compute weights
+            W = (X-Q).colwise().norm();
+            robust_weight(par.f, W, par.p);
+            /// Rotation and translation update
+            Eigen::Affine3d transform;
+            if(par.useDirectSimilarity)
+                transform = directSimilarityEstimator::point_to_point(X, Q, W);
+            else
+                transform = rigidMotionEstimator::point_to_point(X, Q, W);
+            std::cout << "Intermediate transform:\n" << transform.matrix() << std::endl;
+            finalTransform = transform * finalTransform;
+            /// Stopping criteria
+            double stop1 = (X-Xo1).colwise().norm().maxCoeff();
+            Xo1 = X;
+            if(stop1 < par.stop)
+            {
+                std::cout << "stop1 < par.stop:\n" << stop1 << ", " << par.stop << std::endl;
+                break;
+            }
+        }
+        /// Stopping criteria
+        double stop2 = (X-Xo2).colwise().norm().maxCoeff();
+        Xo2 = X;
+        if(stop2 < par.stop)
+        {
+            std::cout << "stop2 < par.stop:\n" << stop2 << ", " << par.stop << std::endl;
+            std::cout << "icp iteration:\n" << icp << std::endl;
+            break;
+        }
+    }
+    return finalTransform;
+}
+/// Reweighted ICP with point to plane
+/// @param Source (one 3D point per column)
+/// @param Target (one 3D point per column)
+/// @param Target normals (one 3D normal per column)
+/// @param Parameters
+template <typename Derived1, typename Derived2, typename Derived3>
+Eigen::Affine3d point_to_plane(Eigen::MatrixBase<Derived1>& X,
+                    Eigen::MatrixBase<Derived2>& Y,
+                    Eigen::MatrixBase<Derived3>& N,
+                    Parameters par = Parameters()) {
+    /// Build kd-tree
+    nanoflann::KDTreeAdaptor<Eigen::MatrixBase<Derived2>, 3, nanoflann::metric_L2_Simple> kdtree(Y);
+    /// Buffers
+    Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, X.cols());
+    Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, X.cols());
+    Eigen::VectorXd W = Eigen::VectorXd::Zero(X.cols());
+    Eigen::Matrix3Xd Xo1 = X;
+    Eigen::Matrix3Xd Xo2 = X;
+    Eigen::Affine3d finalTransform = Eigen::Affine3d::Identity();
+    /// ICP
+    for(int icp=0; icp<par.max_icp; ++icp) {
+        /// Find closest point
+        #pragma omp parallel for
+        for(int i=0; i<X.cols(); ++i) {
+            int id = kdtree.closest(X.col(i).data());
+            Qp.col(i) = Y.col(id);
+            Qn.col(i) = N.col(id);
+        }
+        /// Computer rotation and translation
+        for(int outer=0; outer<par.max_outer; ++outer) {
+            /// Compute weights
+            W = (Qn.array()*(X-Qp).array()).colwise().sum().abs().transpose();
+            robust_weight(par.f, W, par.p);
+            /// Rotation and translation update
+            Eigen::Affine3d transform = rigidMotionEstimator::point_to_plane(X, Qp, Qn, W);
+            finalTransform = transform * finalTransform;
+            /// Stopping criteria
+            double stop1 = (X-Xo1).colwise().norm().maxCoeff();
+            Xo1 = X;
+            if(stop1 < par.stop) break;
+        }
+        /// Stopping criteria
+        double stop2 = (X-Xo2).colwise().norm().maxCoeff() ;
+        Xo2 = X;
+        if(stop2 < par.stop) break;
+    }
+    return finalTransform;
+}
+
+}
+}
+}