Added overloads

Former-commit-id: 742258c3ae881c7f3a1d335f777905175c43a4d4

include/igl/procrustes.cpp

@@ -9,13 +9,15 @@
 #include "polar_svd.h"
 #include "polar_dec.h"
  
-template <typename DerivedV, typename Scalar, int DIM, int TType>
+template <typename DerivedV, typename Scalar, typename DerivedR, typename DerivedT>
 IGL_INLINE void igl::procrustes(
     const Eigen::PlainObjectBase<DerivedV>& X,
     const Eigen::PlainObjectBase<DerivedV>& Y,
     bool includeScaling,
     bool includeReflections,
-    Eigen::Transform<Scalar,DIM,TType>& T)
+    Scalar& scale,
+    Eigen::PlainObjectBase<DerivedR>& R,
+    Eigen::PlainObjectBase<DerivedT>& t)
 {
    using namespace Eigen;
    assert (X.rows() == Y.rows() && "Same number of points");
@@ -27,9 +29,8 @@ IGL_INLINE void igl::procrustes(
    MatrixXd XC = X.rowwise() - Xmean.transpose();
    MatrixXd YC = Y.rowwise() - Ymean.transpose();
 
-
-   // Determine scale
-   double scale = 1.;
+   // Scale
+   scale = 1.;
    if (includeScaling)
    {
        double scaleX = XC.norm() / XC.rows();
@@ -39,18 +40,71 @@ IGL_INLINE void igl::procrustes(
        assert (abs(XC.norm() / XC.rows() - scaleY) < 1e-8);
    }
 
-
    // Rotation 
    MatrixXd S = XC.transpose() * YC; 
-   Matrix<Scalar,DIM,DIM> Rm,Tm;
+   MatrixXd T;
    if (includeReflections)
-     polar_dec(S,Rm,Tm);
+     polar_dec(S,R,T);
    else
-     polar_svd(S,Rm,Tm);
+     polar_svd(S,R,T);
+   R.transposeInPlace();
 
    // Translation
-   Matrix<Scalar,DIM,1> t = Ymean - scale*Rm*Xmean;
+   t = Ymean - scale*R.transpose()*Xmean;
+}
+
+template <typename DerivedV, typename DerivedR, typename DerivedT>
+IGL_INLINE void igl::procrustes(
+    const Eigen::PlainObjectBase<DerivedV>& X,
+    const Eigen::PlainObjectBase<DerivedV>& Y,
+    bool includeScaling,
+    bool includeReflections,
+    Eigen::PlainObjectBase<DerivedR>& S,
+    Eigen::PlainObjectBase<DerivedT>& t)
+{
+  double scale;
+  procrustes(X,Y,includeScaling,includeReflections,scale,S,t);
+  S *= scale;
+}
+
+
+template <typename DerivedV, typename Scalar, int DIM, int TType>
+IGL_INLINE void igl::procrustes(
+    const Eigen::PlainObjectBase<DerivedV>& X,
+    const Eigen::PlainObjectBase<DerivedV>& Y,
+    bool includeScaling,
+    bool includeReflections,
+    Eigen::Transform<Scalar,DIM,TType>& T)
+{
+  double scale;
+  MatrixXd R;
+  VectorXd t;
+  procrustes(X,Y,includeScaling,includeReflections,scale,R,t);
 
    // Combine
-   T = Translation<Scalar,DIM>(t) * Rm.transpose() * Scaling(scale);
+   T = Translation<Scalar,DIM>(t) * R * Scaling(scale);
+}
+
+template <typename DerivedV, typename DerivedR, typename DerivedT>
+IGL_INLINE void igl::procrustes(
+    const Eigen::PlainObjectBase<DerivedV>& X,
+    const Eigen::PlainObjectBase<DerivedV>& Y,
+    Eigen::PlainObjectBase<DerivedR>& R,
+    Eigen::PlainObjectBase<DerivedT>& t)
+{
+  double scale;
+  procrustes(X,Y,false,false,R,t);
+}
+
+template <typename DerivedV, typename Scalar, typename DerivedT>
+IGL_INLINE void igl::procrustes(
+    const Eigen::PlainObjectBase<DerivedV>& X,
+    const Eigen::PlainObjectBase<DerivedV>& Y,
+    Eigen::Rotation2D<Scalar>& R,
+    Eigen::PlainObjectBase<DerivedT>& t)
+{
+  assert (X.cols() == 2 && Y.cols() == 2 && "Points must have dimension 2");
+  Matrix2d Rmat;
+  procrustes(X,Y,false,false,Rmat,t);
+  R.fromRotationMatrix(Rmat);
 }

include/igl/procrustes.h

@@ -15,16 +15,50 @@
 namespace igl
 {
     // Solve Procrustes problem in d dimensions.
-    // Given two point sets X,Y in R^d find best scale s, rotation/reflection R  and translation t 
+    // Given two point sets X,Y in R^d find best scale s, orthogonal R  and translation t 
     // s.t. |s*X*R + t - Y|^2 is minimized.
     //
     // Example:
     // MatrixXd X, Y; (containing 3d points as rows)
+    // double scale;
+    // MatrixXd R;
+    // VectorXd t;
+    // igl::procrustes(X,Y,true,false,scale,R,t);
+    // R *= scale;
+    // MatrixXd Xprime = (X * R).rowwise() + t.transpose();
+    //
+    // Templates:
+    //    DerivedV point type
+    //    Scalar   scalar type
+    //    DerivedR type of R
+    //    DerivedT type of t
+    // Inputs:
+    //    X  #V by DIM first list of points
+    //    Y  #V by DIM second list of points
+    //    includeScaling  if scaling should be allowed
+    //    includeReflections  if R is allowed to be a reflection
+    // Outputs:
+    //    scale  scaling
+    //    R      orthogonal matrix
+    //    t      translation
+    template <typename DerivedV, typename Scalar, typename DerivedR, typename DerivedT>
+    IGL_INLINE void procrustes(
+        const Eigen::PlainObjectBase<DerivedV>& X,
+        const Eigen::PlainObjectBase<DerivedV>& Y,
+        bool includeScaling,
+        bool includeReflections,
+        Scalar& scale,
+        Eigen::PlainObjectBase<DerivedR>& R,
+        Eigen::PlainObjectBase<DerivedT>& t);
+
+
+    // Same as above but returns Eigen transformation object.
+    //
+    // Example:
+    // MatrixXd X, Y; (containing 3d points as rows)
     // AffineCompact3d T;
     // igl::procrustes(X,Y,true,false,T);
     // MatrixXd Xprime = (X * T.linear()).rowwise() + T.translation().transpose();
-    //     
-    //
     // Templates:
     //    DerivedV point type
     //    Scalar   scalar type
@@ -44,7 +78,34 @@ namespace igl
         bool includeScaling,
         bool includeReflections,
         Eigen::Transform<Scalar,DIM,TType>& T);
-    
+
+
+    // Convenient wrapper that returns S=scale*R instead of scale and R separately
+    template <typename DerivedV, typename DerivedR, typename DerivedT>
+    IGL_INLINE void procrustes(
+        const Eigen::PlainObjectBase<DerivedV>& X,
+        const Eigen::PlainObjectBase<DerivedV>& Y,
+        bool includeScaling,
+        bool includeReflections,
+        Eigen::PlainObjectBase<DerivedR>& S,
+        Eigen::PlainObjectBase<DerivedT>& t);
+
+    // Convenient wrapper for rigid case (no scaling, no reflections)
+    template <typename DerivedV, typename DerivedR, typename DerivedT>
+    IGL_INLINE void procrustes(
+        const Eigen::PlainObjectBase<DerivedV>& X,
+        const Eigen::PlainObjectBase<DerivedV>& Y,
+        Eigen::PlainObjectBase<DerivedR>& R,
+        Eigen::PlainObjectBase<DerivedT>& t);
+
+    // Convenient wrapper for 2D case.
+    template <typename DerivedV, typename Scalar, typename DerivedT>
+    IGL_INLINE void procrustes(
+        const Eigen::PlainObjectBase<DerivedV>& X,
+        const Eigen::PlainObjectBase<DerivedV>& Y,
+        Eigen::Rotation2D<Scalar>& R,
+        Eigen::PlainObjectBase<DerivedT>& t);
+
 }
 
 #ifndef IGL_STATIC_LIBRARY