iterative closest point alg, tutoiral, test

include/igl/iterative_closest_point.cpp

@@ -0,0 +1,123 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#include "iterative_closest_point.h"
+#include "AABB.h"
+#include "per_face_normals.h"
+#include "matlab_format.h"
+#include "slice.h"
+#include "random_points_on_mesh.h"
+#include "rigid_alignment.h"
+#include "writeOBJ.h"
+#include "writeDMAT.h"
+#include <cassert>
+#include <iostream>
+
+template <
+  typename DerivedVX,
+  typename DerivedFX,
+  typename DerivedVY,
+  typename DerivedFY,
+  typename DerivedR,
+  typename Derivedt
+  >
+IGL_INLINE void igl::iterative_closest_point(
+  const Eigen::MatrixBase<DerivedVX> & VX,
+  const Eigen::MatrixBase<DerivedFX> & FX,
+  const Eigen::MatrixBase<DerivedVY> & VY,
+  const Eigen::MatrixBase<DerivedFY> & FY,
+  const int num_samples,
+  const int max_iters,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<Derivedt> & t)
+{
+
+  assert(VX.cols() == 3 && "X should be a mesh in 3D");
+  assert(VY.cols() == 3 && "Y should be a mesh in 3D");
+
+  typedef typename DerivedVX::Scalar Scalar;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
+  typedef Eigen::Matrix<Scalar,3,3> Matrix3S;
+  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
+
+  // Precompute BVH on Y
+  AABB<DerivedVY,3> Ytree;
+  Ytree.init(VY,FY);
+  MatrixXS NY;
+  per_face_normals(VY,FY,NY);
+  return iterative_closest_point(
+    VX,FX,VY,FY,Ytree,NY,num_samples,max_iters,R,t);
+}
+
+template <
+  typename DerivedVX,
+  typename DerivedFX,
+  typename DerivedVY,
+  typename DerivedFY,
+  typename DerivedNY,
+  typename DerivedR,
+  typename Derivedt
+  >
+IGL_INLINE void igl::iterative_closest_point(
+  const Eigen::MatrixBase<DerivedVX> & VX,
+  const Eigen::MatrixBase<DerivedFX> & FX,
+  const Eigen::MatrixBase<DerivedVY> & VY,
+  const Eigen::MatrixBase<DerivedFY> & FY,
+  const igl::AABB<DerivedVY,3> & Ytree, 
+  const Eigen::MatrixBase<DerivedNY> & NY,
+  const int num_samples,
+  const int max_iters,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<Derivedt> & t)
+{
+  typedef typename DerivedVX::Scalar Scalar;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
+  typedef Eigen::Matrix<Scalar,3,3> Matrix3S;
+  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
+  R.setIdentity(3,3);
+  t.setConstant(1,3,0);
+
+  for(int iter = 0;iter<max_iters;iter++)
+  {
+    // Sample random points on X
+    MatrixXS X;
+    {
+      Eigen::VectorXi XI;
+      Eigen::MatrixXd B;
+      MatrixXS VXRT = (VX*R).rowwise()+t;
+
+      random_points_on_mesh(num_samples,VXRT,FX,B,XI,X);
+    }
+    // Compute closest point
+    Eigen::VectorXi I;
+    MatrixXS P;
+    {
+      VectorXS sqrD;
+      Ytree.squared_distance(VY,FY,X,sqrD,I,P);
+    }
+    // Use better normals?
+    MatrixXS N;
+    slice(NY,I,1,N);
+    //MatrixXS N = (X - P).rowwise().normalized();
+    // fit rotation,translation
+    Matrix3S Rup;
+    RowVector3S tup;
+    // Note: Should try out Szymon Rusinkiewicz's new symmetric icp
+    rigid_alignment(X,P,N,Rup,tup);
+    // update running rigid transformation
+    R = (R*Rup).eval();
+    t = (t*Rup + tup).eval();
+    // Better stopping condition?
+  }
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+template void igl::iterative_closest_point<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, int, int, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
+#endif

include/igl/iterative_closest_point.h

@@ -0,0 +1,83 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#ifndef IGL_ITERATIVE_CLOSEST_POINT_H
+#define IGL_ITERATIVE_CLOSEST_POINT_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+#include "AABB.h"
+
+namespace igl
+{
+  // Solve for the rigid transformation that places mesh X onto mesh Y using the
+  // iterative closest point method. In particular, optimize:
+  //
+  // min      ∫_X inf ‖x*R+t - y‖² dx
+  // R∈SO(3)      y∈Y
+  // t∈R³
+  //
+  // Typically optimization strategies include using Gauss Newton
+  // ("point-to-plane" linearization) and stochastic descent (sparse random
+  // sampling each iteration).
+  //
+  // Inputs:
+  //   VX  #VX by 3 list of mesh X vertices
+  //   FX  #FX by 3 list of mesh X triangle indices into rows of VX
+  //   VY  #VY by 3 list of mesh Y vertices
+  //   FY  #FY by 3 list of mesh Y triangle indices into rows of VY
+  //   num_samples  number of random samples to use (larger --> more accurate,
+  //     but also more suceptible to sticking to local minimum)
+  // Outputs:
+  //   R  3x3 rotation matrix so that (VX*R+t,FX) ~~ (VY,FY)
+  //   t  1x3 translation row vector
+  template <
+    typename DerivedVX,
+    typename DerivedFX,
+    typename DerivedVY,
+    typename DerivedFY,
+    typename DerivedR,
+    typename Derivedt
+    >
+  IGL_INLINE void iterative_closest_point(
+    const Eigen::MatrixBase<DerivedVX> & VX,
+    const Eigen::MatrixBase<DerivedFX> & FX,
+    const Eigen::MatrixBase<DerivedVY> & VY,
+    const Eigen::MatrixBase<DerivedFY> & FY,
+    const int num_samples,
+    const int max_iters,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<Derivedt> & t);
+  // Inputs:
+  //   Ytree  precomputed AABB tree for accelerating closest point queries
+  //   NY  #FY by 3 list of precomputed unit face normals
+  template <
+    typename DerivedVX,
+    typename DerivedFX,
+    typename DerivedVY,
+    typename DerivedFY,
+    typename DerivedNY,
+    typename DerivedR,
+    typename Derivedt
+    >
+  IGL_INLINE void iterative_closest_point(
+    const Eigen::MatrixBase<DerivedVX> & VX,
+    const Eigen::MatrixBase<DerivedFX> & FX,
+    const Eigen::MatrixBase<DerivedVY> & VY,
+    const Eigen::MatrixBase<DerivedFY> & FY,
+    const igl::AABB<DerivedVY,3> & Ytree, 
+    const Eigen::MatrixBase<DerivedNY> & NY,
+    const int num_samples,
+    const int max_iters,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<Derivedt> & t);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "iterative_closest_point.cpp"
+#endif
+
+#endif

include/igl/rigid_alignment.cpp

@@ -0,0 +1,98 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#include "rigid_alignment.h"
+#include "polar_svd.h"
+#include "matlab_format.h"
+#include <Eigen/Sparse>
+#include <Eigen/Cholesky>
+#include <vector>
+#include <iostream>
+
+template <
+  typename DerivedX,
+  typename DerivedP,
+  typename DerivedN,
+  typename DerivedR,
+  typename Derivedt
+>
+IGL_INLINE void igl::rigid_alignment(
+  const Eigen::MatrixBase<DerivedX> & _X,
+  const Eigen::MatrixBase<DerivedP> & P,
+  const Eigen::MatrixBase<DerivedN> & N,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<Derivedt> & t)
+{
+  typedef typename DerivedX::Scalar Scalar;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
+  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
+  typedef Eigen::Matrix<Scalar,3,3> Matrix3S;
+  const int k = _X.rows();
+  VectorXS Z = VectorXS::Zero(k,1);
+  VectorXS I = VectorXS::Ones(k,1);
+
+  DerivedX X = _X;
+  R = DerivedR::Identity(3,3);
+  t = Derivedt::Zero(1,3);
+  // See gptoolbox, each iter could be O(1) instead of O(k)
+  const int max_iters = 5;
+  for(int iters = 0;iters<max_iters;iters++)
+  {
+    MatrixXS A(k*3,6);
+    A <<
+               Z, X.col(2),-X.col(1),I,Z,Z,
+       -X.col(2),        Z, X.col(0),Z,I,Z,
+        X.col(1),-X.col(0),        Z,Z,Z,I;
+    VectorXS B(k*3,1);
+    B<<
+      P.col(0)-X.col(0),
+      P.col(1)-X.col(1),
+      P.col(2)-X.col(2);
+    std::vector<Eigen::Triplet<Scalar> > NNIJV;
+    for(int i = 0;i<k;i++)
+    {
+      for(int c = 0;c<3;c++)
+      {
+        NNIJV.emplace_back(i,i+k*c,N(i,c));
+      }
+    }
+    Eigen::SparseMatrix<Scalar> NN(k,k*3);
+    NN.setFromTriplets(NNIJV.begin(),NNIJV.end());
+    A = (NN * A).eval();
+    B = (NN * B).eval();
+    VectorXS u = (A.transpose() * A).ldlt().solve(A.transpose() * B);
+    Derivedt ti = u.tail(3).transpose();
+
+    Matrix3S W;
+    W<<
+          0, u(2),-u(1),
+      -u(2),    0, u(0),
+       u(1),-u(0),    0;
+    // strayed from a perfect rotation. Correct it.
+    const double x = u.head(3).stableNorm();
+    DerivedR Ri;
+    if(x == 0)
+    {
+      Ri = DerivedR::Identity(3,3);
+    }else
+    {
+      Ri = 
+        DerivedR::Identity(3,3) + 
+        sin(x)/x*W + 
+        (1.0-cos(x))/(x*x)*W*W;
+    }
+    
+    R = (R*Ri).eval();
+    t = (t*Ri + ti).eval();
+    X = ((_X*R).rowwise()+t).eval();
+  }
+}
+
+#ifdef IGL_STATIC_LIBRARY
+// Explicit template instantiation
+template void igl::rigid_alignment<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
+#endif

include/igl/rigid_alignment.h

@@ -0,0 +1,50 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#ifndef RIGID_ALIGNMENT_H
+#define RIGID_ALIGNMENT_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+
+namespace igl
+{
+  // Find the rigid transformation that best aligns the 3D points X to their
+  // corresponding points P with associated normals N.
+  //
+  //  min       ‖(X*R+t-P)'N‖²
+  //  R∈SO(3)
+  //  t∈R³
+  //
+  // Inputs:
+  //   X  #X by 3 list of query points
+  //   P  #X by 3 list of corresponding (e.g., closest) points
+  //   N  #X by 3 list of unit normals for each row in P
+  // Outputs:
+  //   R  3 by 3 rotation matrix
+  //   t  1 by 3 translation vector
+  //
+  //   See also: icp
+  template <
+    typename DerivedX,
+    typename DerivedP,
+    typename DerivedN,
+    typename DerivedR,
+    typename Derivedt
+  >
+  IGL_INLINE void rigid_alignment(
+    const Eigen::MatrixBase<DerivedX> & X,
+    const Eigen::MatrixBase<DerivedP> & P,
+    const Eigen::MatrixBase<DerivedN> & N,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<Derivedt> & t);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "rigid_alignment.cpp"
+#endif
+
+#endif 

tests/include/igl/iterative_closest_point.cpp

@@ -0,0 +1,32 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#include <test_common.h>
+#include <igl/iterative_closest_point.h>
+
+
+TEST_CASE("iterative_closest_point: identity","[igl]")
+{
+  const auto test_case = [](const std::string &param)
+  {
+    Eigen::MatrixXd VX,VY;
+    Eigen::MatrixXi FX,FY;
+    // Load example mesh: GetParam() will be name of mesh file
+    test_common::load_mesh(param, VY, FY);
+    VX = VY;
+    FX = FY;
+    // Single iteration should find a identity
+    srand(0);
+    Eigen::Matrix3d R;
+    Eigen::RowVector3d t;
+    igl::iterative_closest_point(VX,FX,VY,FY,1000,1,R,t);
+    test_common::assert_near(R,Eigen::Matrix3d::Identity(),1e-12);
+    test_common::assert_near(t,Eigen::RowVector3d::Zero(),1e-12);
+  };
+
+  test_common::run_test_cases(test_common::all_meshes(), test_case);
+}

tests/include/igl/rigid_alignment.cpp

@@ -0,0 +1,34 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#include <test_common.h>
+#include <igl/rigid_alignment.h>
+#include <igl/matlab_format.h>
+#include <iostream>
+
+
+TEST_CASE("rigid_alignment: identity", "[igl]")
+{
+  const Eigen::MatrixXd X = (Eigen::MatrixXd(10,3)<<
+    0.814724,0.157613,0.655741,
+    0.905792,0.970593,0.035712,
+    0.126987,0.957167,0.849129,
+    0.913376,0.485376,0.933993,
+    0.632359,0.800280,0.678735,
+    0.097540,0.141886,0.757740,
+    0.278498,0.421761,0.743132,
+    0.546882,0.915736,0.392227,
+    0.957507,0.792207,0.655478,
+    0.964889,0.959492,0.171187).finished();
+  const Eigen::MatrixXd Y = X;
+  const Eigen::MatrixXd N = (Y.array()-0.5).matrix().rowwise().normalized();
+  Eigen::Matrix3d R;
+  Eigen::RowVector3d t;
+  igl::rigid_alignment(X,Y,N,R,t);
+  test_common::assert_near(R,Eigen::Matrix3d::Identity(),1e-12);
+  test_common::assert_near(t,Eigen::RowVector3d::Zero(),1e-12);
+}

tutorial/718_IterativeClosestPoint/CMakeLists.txt

@@ -0,0 +1,5 @@
+get_filename_component(PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR} NAME)
+project(${PROJECT_NAME})
+
+add_executable(${PROJECT_NAME}_bin main.cpp)
+target_link_libraries(${PROJECT_NAME}_bin igl::core igl::opengl igl::opengl_glfw tutorials)

tutorial/718_IterativeClosestPoint/main.cpp

@@ -0,0 +1,108 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2019 Alec Jacobson <[email protected]>
+// 
+// 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 http://mozilla.org/MPL/2.0/.
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <igl/read_triangle_mesh.h>
+#include <igl/opengl/glfw/Viewer.h>
+#include <igl/iterative_closest_point.h>
+#include <igl/random_dir.h>
+#include <igl/PI.h>
+#include <igl/AABB.h>
+#include <igl/per_face_normals.h>
+#include <igl/avg_edge_length.h>
+#include <igl/per_vertex_normals.h>
+#include <igl/barycenter.h>
+#include <igl/slice.h>
+#include <igl/slice_mask.h>
+#include <iostream>
+
+int main(int argc, char * argv[])
+{
+  Eigen::MatrixXd OVX,VX,VY;
+  Eigen::MatrixXi FX,FY;
+  igl::read_triangle_mesh( argc>1?argv[1]: TUTORIAL_SHARED_PATH "/decimated-max.obj",VY,FY);
+  const double bbd = (VY.colwise().maxCoeff()-VY.colwise().minCoeff()).norm();
+  FX = FY;
+  {
+    // sprinkle a noise so that we can see z-fighting when the match is perfect.
+    const double h = igl::avg_edge_length(VY,FY);
+    OVX = VY + 1e-2*h*Eigen::MatrixXd::Random(VY.rows(),VY.cols());
+  }
+  
+  VX = OVX;
+
+  igl::AABB<Eigen::MatrixXd,3> Ytree;
+  Ytree.init(VY,FY);
+  Eigen::MatrixXd NY;
+  igl::per_face_normals(VY,FY,NY);
+
+  igl::opengl::glfw::Viewer v;
+  std::cout<<R"(
+  [space]  conduct a single iterative closest point iteration
+  R,r      reset to a random orientation and offset
+)";
+
+  const auto apply_random_rotation = [&]()
+  {
+    const Eigen::Matrix3d R = Eigen::AngleAxisd(
+      2.*igl::PI*(double)rand()/RAND_MAX*0.3, igl::random_dir()).matrix();
+    const Eigen::RowVector3d cen = 
+      0.5*(VY.colwise().maxCoeff()+VY.colwise().minCoeff());
+    VX = ((OVX*R).rowwise()+(cen-cen*R)).eval();
+  };
+  const auto single_iteration = [&]()
+  {
+    ////////////////////////////////////////////////////////////////////////
+    // Perform single iteration of ICP method
+    ////////////////////////////////////////////////////////////////////////
+    Eigen::Matrix3d R;
+    Eigen::RowVector3d t;
+    igl::iterative_closest_point(VX,FX,VY,FY,Ytree,NY,1000,1,R,t);
+    VX = ((VX*R).rowwise()+t).eval();
+    v.data().set_mesh(VX,FX);
+    v.data().compute_normals();
+  };
+  v.callback_pre_draw = [&](igl::opengl::glfw::Viewer &)->bool
+  {
+    if(v.core().is_animating)
+    {
+      single_iteration();
+    }
+    return false;
+  };
+  v.callback_key_pressed = 
+    [&](igl::opengl::glfw::Viewer &,unsigned char key,int)->bool
+  {
+    switch(key)
+    {
+      case ' ':
+      {
+        v.core().is_animating = false;
+        single_iteration();
+        return true;
+      }
+      case 'R':
+      case 'r':
+        // Random rigid transformation
+        apply_random_rotation();
+        v.data().set_mesh(VX,FX);
+        v.data().compute_normals();
+        return true;
+        break;
+    }
+    return false;
+  };
+
+  v.data().set_mesh(VY,FY);
+  v.data().set_colors(Eigen::RowVector3d(1,1,1));
+  v.data().show_lines = false;
+  v.append_mesh();
+  v.data().set_mesh(VX,FX);
+  v.data().show_lines = false;
+  v.launch();
+}

tutorial/CMakeLists.txt

@@ -155,4 +155,5 @@ if(TUTORIALS_CHAPTER7)
   endif()
   add_subdirectory("715_MeshImplicitFunction")
   add_subdirectory("716_HeatGeodesics")
+  add_subdirectory("718_IterativeClosestPoint")
 endif()