Merge pull request #29 from stefanbrugger/procrustes2

Procrustes

Former-commit-id: 530ef547a3457993a2544ed0036a9bea5895c02b

include/igl/fit_rigid.cpp

@@ -6,7 +6,7 @@
 // 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 "fit_rigid.h"
-#include "polar_svd.h"
+#include "procrustes.h"
 
 IGL_INLINE void igl::fit_rigid(
   const Eigen::MatrixXd & A,
@@ -14,12 +14,7 @@ IGL_INLINE void igl::fit_rigid(
   Eigen::Rotation2Dd & R,
   Eigen::RowVector2d & t)
 {
-  // build covariance matrix
-  const auto & Amean = A.colwise().mean();
-  const auto & Bmean = B.colwise().mean();
-  const auto & S = (B.rowwise() - Bmean).transpose() * (A.rowwise() - Amean);
-  Eigen::Matrix2d Rm,Tm;
-  polar_svd(S.eval(),Rm,Tm);
-  R.fromRotationMatrix(Rm);
-  t = Amean - Bmean*Rm;
+  Matrix2d Rmat;
+  procrustes(A,B,Rmat,t);
+  R.fromRotationMatrix(Rmat);
 }

include/igl/fit_rigid.h

@@ -14,6 +14,7 @@
 
 namespace igl
 {
+  // Deprecated: please use procrustes(...) instead.
   // Fit a rigid 
   IGL_INLINE void fit_rigid(
     const Eigen::MatrixXd & A,

include/igl/procrustes.cpp

@@ -0,0 +1,110 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2014 Stefan Brugger <stefanbrugger@gmail.com>
+// 
+// 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 "procrustes.h"
+#include "polar_svd.h"
+#include "polar_dec.h"
+ 
+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,
+    Scalar& scale,
+    Eigen::PlainObjectBase<DerivedR>& R,
+    Eigen::PlainObjectBase<DerivedT>& t)
+{
+   using namespace Eigen;
+   assert (X.rows() == Y.rows() && "Same number of points");
+   assert(X.cols() == Y.cols() && "Points have same dimensions");
+
+   // Center data
+   const VectorXd Xmean = X.colwise().mean();      
+   const VectorXd Ymean = Y.colwise().mean();      
+   MatrixXd XC = X.rowwise() - Xmean.transpose();
+   MatrixXd 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 (abs(XC.norm() / XC.rows() - scaleY) < 1e-8);
+   }
+
+   // Rotation 
+   MatrixXd S = XC.transpose() * YC; 
+   MatrixXd T;
+   if (includeReflections)
+     polar_dec(S,R,T);
+   else
+     polar_svd(S,R,T);
+   R.transposeInPlace();
+
+   // Translation
+   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) * 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

@@ -0,0 +1,115 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2014 Stefan Brugger <stefanbrugger@gmail.com>
+// 
+// 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_PROCRUSTES_H
+#define IGL_PROCRUSTES_H
+#include "igl_inline.h"
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+namespace igl
+{
+    // Solve Procrustes problem in d dimensions.
+    // 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
+    //    DIM      point dimension
+    //    TType    type of transformation (Isometry,Affine,AffineCompact,Projective)
+    // 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:
+    //    T  transformation that minimizes error    
+    template <typename DerivedV, typename Scalar, int DIM, int TType>
+    IGL_INLINE void procrustes(
+        const Eigen::PlainObjectBase<DerivedV>& X,
+        const Eigen::PlainObjectBase<DerivedV>& Y,
+        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
+    #include "procrustes.cpp"
+#endif
+
+#endif