Fix scale and reflections in Procrustes solver (#2226) [ci skip]

* Allow returning reflections from polar_dec and polar_svd

* Fix scale in Procrustes solver

* Fix reflection case in Procrustes solver

* Add unit test for Procrustes

* fix overloads for backwards comp

* missing templates on linux

---------

Co-authored-by: Alec Jacobson <[email protected]>
Co-authored-by: Alec Jacobson <[email protected]>

include/igl/polar_dec.cpp

@@ -26,6 +26,7 @@ template <
   typename DerivedV>
 IGL_INLINE void igl::polar_dec(
   const Eigen::PlainObjectBase<DerivedA> & A,
+  const bool includeReflections,
   Eigen::PlainObjectBase<DerivedR> & R,
   Eigen::PlainObjectBase<DerivedT> & T,
   Eigen::PlainObjectBase<DerivedU> & U,
@@ -44,7 +45,7 @@ IGL_INLINE void igl::polar_dec(
   if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
   {
     cout<<"resorting to svd 1..."<<endl;
-    return polar_svd(A,R,T,U,S,V);
+    return polar_svd(A,includeReflections,R,T,U,S,V);
   }
 
   S = eig.eigenvalues().cwiseSqrt();
@@ -58,7 +59,7 @@ IGL_INLINE void igl::polar_dec(
   V = V.rowwise().reverse().eval();
   U = U.rowwise().reverse().eval() * S.asDiagonal().inverse();
 
-  if(R.determinant() < 0)
+  if(!includeReflections && R.determinant() < 0)
   {
     // Annoyingly the .eval() is necessary
     auto W = V.eval();
@@ -71,7 +72,7 @@ IGL_INLINE void igl::polar_dec(
   if(std::fabs(R.squaredNorm()-3.) > th)
   {
     cout<<"resorting to svd 2..."<<endl;
-    return polar_svd(A,R,T,U,S,V);
+    return polar_svd(A,includeReflections,R,T,U,S,V);
   }
 }
 
@@ -81,17 +82,54 @@ template <
   typename DerivedT>
 IGL_INLINE void igl::polar_dec(
   const Eigen::PlainObjectBase<DerivedA> & A,
+  const bool includeReflections,
   Eigen::PlainObjectBase<DerivedR> & R,
   Eigen::PlainObjectBase<DerivedT> & T)
 {
   DerivedA U;
   DerivedA V;
   Eigen::Matrix<typename DerivedA::Scalar,DerivedA::RowsAtCompileTime,1> S;
-  return igl::polar_dec(A,R,T,U,S,V);
+  return igl::polar_dec(A,includeReflections,R,T,U,S,V);
+}
+
+
+template <
+  typename DerivedA,
+  typename DerivedR,
+  typename DerivedT,
+  typename DerivedU,
+  typename DerivedS,
+  typename DerivedV>
+IGL_INLINE void igl::polar_dec(
+  const Eigen::PlainObjectBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<DerivedT> & T,
+  Eigen::PlainObjectBase<DerivedU> & U,
+  Eigen::PlainObjectBase<DerivedS> & S,
+  Eigen::PlainObjectBase<DerivedV> & V)
+{
+  // do **not** include reflections (for backwards compatibility)
+  return igl::polar_dec(A,false,R,T,U,S,V);
+}
+
+template <
+  typename DerivedA,
+  typename DerivedR,
+  typename DerivedT>
+IGL_INLINE void igl::polar_dec(
+  const Eigen::PlainObjectBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<DerivedT> & T)
+{
+  // do **not** include reflections (for backwards compatibility)
+  return igl::polar_dec(A,false,R,T);
 }
 
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template instantiation
-template  void igl::polar_dec<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, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::polar_dec<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, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<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, -1, 0, -1, -1> >&);
+template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
 template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
 #endif

include/igl/polar_dec.h

@@ -15,6 +15,7 @@ namespace igl
   /// Computes the polar decomposition (R,T) of a matrix A
   ///
   /// @param[in] A  3 by 3 matrix to be decomposed
+  /// @param[in] includeReflections  Whether to force R to be a rotation, or allow it to be a reflection
   /// @param[out] R  3 by 3 orthonormal matrix part of decomposition
   /// @param[out] T  3 by 3 stretch matrix part of decomposition
   /// @param[out] U  3 by 3 left-singular vectors
@@ -22,6 +23,33 @@ namespace igl
   /// @param[out] V  3 by 3 right-singular vectors
   ///
   ///
+  template <
+    typename DerivedA,
+    typename DerivedR,
+    typename DerivedT,
+    typename DerivedU,
+    typename DerivedS,
+    typename DerivedV>
+  IGL_INLINE void polar_dec(
+    const Eigen::PlainObjectBase<DerivedA> & A,
+    bool includeReflections,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<DerivedT> & T,
+    Eigen::PlainObjectBase<DerivedU> & U,
+    Eigen::PlainObjectBase<DerivedS> & S,
+    Eigen::PlainObjectBase<DerivedV> & V);
+  /// \overload
+  template <
+    typename DerivedA,
+    typename DerivedR,
+    typename DerivedT>
+  IGL_INLINE void polar_dec(
+    const Eigen::PlainObjectBase<DerivedA> & A,
+    const bool includeReflections,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<DerivedT> & T);
+  /// \overload
+  /// \brief For backward compatibility, these set includeReflections = false.
   template <
     typename DerivedA,
     typename DerivedR,

include/igl/polar_svd.cpp

@@ -17,6 +17,7 @@ template <
   typename DerivedT>
 IGL_INLINE void igl::polar_svd(
   const Eigen::MatrixBase<DerivedA> & A,
+  const bool includeReflections,
   Eigen::PlainObjectBase<DerivedR> & R,
   Eigen::PlainObjectBase<DerivedT> & T)
 {
@@ -28,7 +29,7 @@ IGL_INLINE void igl::polar_svd(
   MatA U;
   MatA V;
   Eigen::Matrix<typename DerivedA::Scalar,DerivedA::RowsAtCompileTime,1> S;
-  return igl::polar_svd(A,R,T,U,S,V);
+  return igl::polar_svd(A,includeReflections,R,T,U,S,V);
 }
 
 template <
@@ -40,6 +41,7 @@ template <
   typename DerivedV>
 IGL_INLINE void igl::polar_svd(
   const Eigen::MatrixBase<DerivedA> & A,
+  const bool includeReflections,
   Eigen::PlainObjectBase<DerivedR> & R,
   Eigen::PlainObjectBase<DerivedT> & T,
   Eigen::PlainObjectBase<DerivedU> & U,
@@ -60,7 +62,7 @@ IGL_INLINE void igl::polar_svd(
   R = U*V.transpose();
   const auto & SVT = S.asDiagonal() * V.adjoint();
   // Check for reflection
-  if(R.determinant() < 0)
+  if(!includeReflections && R.determinant() < 0)
   {
     // Annoyingly the .eval() is necessary
     auto W = V.eval();
@@ -73,9 +75,38 @@ IGL_INLINE void igl::polar_svd(
   }
 }
 
+template <
+  typename DerivedA,
+  typename DerivedR,
+  typename DerivedT,
+  typename DerivedU,
+  typename DerivedS,
+  typename DerivedV>
+IGL_INLINE void igl::polar_svd(
+  const Eigen::MatrixBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<DerivedT> & T,
+  Eigen::PlainObjectBase<DerivedU> & U,
+  Eigen::PlainObjectBase<DerivedS> & S,
+  Eigen::PlainObjectBase<DerivedV> & V)
+{
+  return igl::polar_svd(A,false,R,T,U,S,V);
+}
+/// \overload
+template <
+  typename DerivedA,
+  typename DerivedR,
+  typename DerivedT>
+IGL_INLINE void igl::polar_svd(
+  const Eigen::MatrixBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedR> & R,
+  Eigen::PlainObjectBase<DerivedT> & T)
+{
+  return igl::polar_svd(A,false,R,T);
+}
+
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template instantiation
-// generated by autoexplicit.sh
 template void igl::polar_svd<Eigen::Transpose<Eigen::Matrix<double, 3, 3, 0, 3, 3> >, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Transpose<Eigen::Matrix<double, 3, 3, 0, 3, 3> > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
 template void igl::polar_svd<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, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
 template void igl::polar_svd<Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&);
@@ -89,4 +120,5 @@ template void igl::polar_svd<Eigen::Matrix<float, 2, 2, 0, 2, 2>, Eigen::Matrix<
 template void igl::polar_svd<Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 1, 0, 2, 1>, Eigen::Matrix<double, 2, 2, 0, 2, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&);
 template void igl::polar_svd<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::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
 template void igl::polar_svd<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, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
+template void igl::polar_svd<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(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, -1, 0, -1, -1> >&);
 #endif

include/igl/polar_svd.h

@@ -16,6 +16,7 @@ namespace igl
   /// value decomposition
   ///
   /// @param[in] A  3 by 3 matrix to be decomposed
+  /// @param[in] includeReflections  Whether to force R to be a rotation, or allow it to be a reflection
   /// @param[out] R  3 by 3 rotation matrix part of decomposition (**always rotataion**)
   /// @param[out] T  3 by 3 stretch matrix part of decomposition
   /// @param[out] U  3 by 3 left-singular vectors
@@ -23,6 +24,33 @@ namespace igl
   /// @param[out] V  3 by 3 right-singular vectors
   ///
   ///
+  template <
+    typename DerivedA,
+    typename DerivedR,
+    typename DerivedT,
+    typename DerivedU,
+    typename DerivedS,
+    typename DerivedV>
+  IGL_INLINE void polar_svd(
+    const Eigen::MatrixBase<DerivedA> & A,
+    bool includeReflections,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<DerivedT> & T,
+    Eigen::PlainObjectBase<DerivedU> & U,
+    Eigen::PlainObjectBase<DerivedS> & S,
+    Eigen::PlainObjectBase<DerivedV> & V);
+  /// \overload
+  template <
+    typename DerivedA,
+    typename DerivedR,
+    typename DerivedT>
+  IGL_INLINE void polar_svd(
+    const Eigen::MatrixBase<DerivedA> & A,
+    const bool includeReflections,
+    Eigen::PlainObjectBase<DerivedR> & R,
+    Eigen::PlainObjectBase<DerivedT> & T);
+  /// \overload
+  /// \brief For backward compatibility, these set includeReflections = false.
   template <
     typename DerivedA,
     typename DerivedR,

include/igl/procrustes.cpp

@@ -6,7 +6,6 @@
 // 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 "procrustes.h"
-#include "polar_svd.h"
 #include "polar_dec.h"
 
 template <
@@ -18,8 +17,8 @@ template <
 IGL_INLINE void igl::procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Scalar& scale,
     Eigen::PlainObjectBase<DerivedR>& R,
     Eigen::PlainObjectBase<DerivedT>& t)
@@ -36,28 +35,17 @@ IGL_INLINE void igl::procrustes(
   Matrix<typename DerivedY::Scalar, Dynamic, Dynamic> YC
       = Y.rowwise() - Ymean.transpose();
 
-  // Scale
-  scale = 1.;
-  if (includeScaling)
-  {
-     double scaleX = XC.norm() / XC.rows();
-     double scaleY = YC.norm() / YC.rows();
-     scale = scaleY/scaleX;
-     XC *= scale;
-     assert (std::abs(XC.norm() / XC.rows() - scaleY) < 1e-8);
-  }
-
   // Rotation
   Matrix<typename DerivedX::Scalar, Dynamic, Dynamic> S = XC.transpose() * YC;
   Matrix<typename DerivedT::Scalar, Dynamic, Dynamic> T;
-  if (includeReflections)
-  {
-    polar_dec(S,R,T);
-  }else
+  polar_dec(S, includeReflections, R, T);
+
+  // Scale
+  scale = 1.;
+  if (includeScaling)
   {
-    polar_svd(S,R,T);
+      scale = (R.transpose() * S).trace() / (XC.array() * XC.array()).sum();
   }
-//  R.transposeInPlace();
 
   // Translation
   t = Ymean - scale*R.transpose()*Xmean;
@@ -73,8 +61,8 @@ template <
 IGL_INLINE void igl::procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Eigen::Transform<Scalar,DIM,TType>& T)
 {
   using namespace Eigen;
@@ -95,8 +83,8 @@ template <
 IGL_INLINE void igl::procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Eigen::PlainObjectBase<DerivedR>& S,
     Eigen::PlainObjectBase<DerivedT>& t)
 {
@@ -138,5 +126,6 @@ IGL_INLINE void igl::procrustes(
 }
 
 #ifdef IGL_STATIC_LIBRARY
-template void igl::procrustes<Eigen::Matrix<double, 3, 2, 0, 3, 2>, Eigen::Matrix<double, 3, 2, 0, 3, 2>, double, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 3, 2, 0, 3, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 2, 0, 3, 2> > const&, bool, bool, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
+template void igl::procrustes<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, double, Eigen::Matrix<double, 3, 3, 0, 3, 3>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, const bool, const bool, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 3, 0, 3, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
+template void igl::procrustes<Eigen::Matrix<double, 3, 2, 0, 3, 2>, Eigen::Matrix<double, 3, 2, 0, 3, 2>, double, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 3, 2, 0, 3, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 2, 0, 3, 2> > const&, const bool, const bool, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
 #endif

include/igl/procrustes.h

@@ -51,8 +51,8 @@ namespace igl
   IGL_INLINE void procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Scalar& scale,
     Eigen::PlainObjectBase<DerivedR>& R,
     Eigen::PlainObjectBase<DerivedT>& t);
@@ -77,8 +77,8 @@ namespace igl
   IGL_INLINE void procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Eigen::Transform<Scalar,DIM,TType>& T);
   /// \overload
   /// @param[out] S  S=scale*R, instead of scale and R separately
@@ -90,8 +90,8 @@ namespace igl
   IGL_INLINE void procrustes(
     const Eigen::MatrixBase<DerivedX>& X,
     const Eigen::MatrixBase<DerivedY>& Y,
-    bool includeScaling,
-    bool includeReflections,
+    const bool includeScaling,
+    const bool includeReflections,
     Eigen::PlainObjectBase<DerivedR>& S,
     Eigen::PlainObjectBase<DerivedT>& t);
   /// \overload

tests/include/igl/procrustes.cpp

@@ -0,0 +1,69 @@
+#include <test_common.h>
+#include <igl/procrustes.h>
+
+TEST_CASE("procrustes", "[igl]")
+{
+    using namespace igl;
+    Eigen::Matrix<double, Eigen::Dynamic, 3> X(5, 3);
+    X <<
+        -0.7, -1.19, 0.8,
+        0.86, 0.57, -0.19,
+        -0.44, 0.65, -0.14,
+        -1.67, 2.75, 0.65,
+        2.67, -0.41, -1.34;
+    Eigen::Matrix<double, Eigen::Dynamic, 3> Y(5, 3);
+    Y <<
+        -0.32, -0.32, -0.11,
+        0.54, -0.29, -0.2,
+        -0.59, -0.9, -0.45,
+        0.51, 1.18, 0.33,
+        -0.19, -0.82, 0.24;
+
+    Eigen::Matrix<double, Eigen::Dynamic, 3> Xref(5, 3);
+    Eigen::Matrix<double, Eigen::Dynamic, 3> Xhat;
+    double scale;
+    Eigen::Matrix3d R;
+    Eigen::Vector3d t;
+
+    const float eps = 1e-6;
+
+    procrustes(X, Y, /*includeScaling*/ true, /*includeReflections*/ false, scale, R, t);
+    Xhat = (scale * X * R).rowwise() + t.transpose();
+    Xref <<
+        -5.897354483604431152e-01, -1.769234091043472290e-01, -2.173528671264648438e-01,
+        7.268430292606353760e-02, -3.918691575527191162e-01, 8.458440750837326050e-02,
+        2.018260955810546875e-02, -8.983440697193145752e-02, -1.527965068817138672e-01,
+        3.137306272983551025e-01, 5.987602472305297852e-01, -1.497855782508850098e-02,
+        1.331380605697631836e-01, -1.090133190155029297e+00, 1.105435565114021301e-01;
+    test_common::assert_near(Xhat, Xref, eps);
+
+    procrustes(X, Y, /*includeScaling*/ false, /*includeReflections*/ false, scale, R, t);
+    Xhat = (scale * X * R).rowwise() + t.transpose();
+    Xref <<
+        -1.958898901939392090e+00, -5.157226324081420898e-02, -6.409312486648559570e-01,
+        2.679601013660430908e-01, -7.741562128067016602e-01, 3.740924298763275146e-01,
+        9.146496653556823730e-02, 2.411951720714569092e-01, -4.239118695259094238e-01,
+        1.078286409378051758e+00, 2.556046247482299805e+00, 3.939124941825866699e-02,
+        4.711876809597015381e-01, -3.121513128280639648e+00, 4.613594114780426025e-01;
+    test_common::assert_near(Xhat, Xref, eps);
+
+    procrustes(X, Y, /*includeScaling*/ true, /*includeReflections*/ true, scale, R, t);
+    Xhat = (scale * X * R).rowwise() + t.transpose();
+    Xref <<
+        -4.254432320594787598e-01, -2.139694541692733765e-01, -4.958550631999969482e-01,
+        1.176588535308837891e-01, -4.038954973220825195e-01, 1.682620495557785034e-02,
+        -8.266170322895050049e-02, -6.510947644710540771e-02, 9.997285902500152588e-03,
+        2.587105333805084229e-01, 6.245076656341552734e-01, 8.087203651666641235e-02,
+        8.173567056655883789e-02, -1.091533184051513672e+00, 1.981595158576965332e-01;
+    test_common::assert_near(Xhat, Xref, eps);
+
+    procrustes(X, Y, /*includeScaling*/ false, /*includeReflections*/ true, scale, R, t);
+    Xhat = (scale * X * R).rowwise() + t.transpose();
+    Xref <<
+        -1.385619997978210449e+00, -1.769195646047592163e-01, -1.554054498672485352e+00,
+        4.127053320407867432e-01, -8.058047294616699219e-01, 1.435410976409912109e-01,
+        -2.505982220172882080e-01, 3.159871101379394531e-01, 1.209291219711303711e-01,
+        8.797571659088134766e-01, 2.599454402923583984e+00, 3.556103110313415527e-01,
+        2.937560975551605225e-01, -3.082717418670654297e+00, 7.439738512039184570e-01;
+    test_common::assert_near(Xhat, Xref, eps);
+}