procrustes solver

Former-commit-id: d288fe46bc0fd2985485107433dfaa3da71c92be

include/igl/procrustes.cpp

@@ -0,0 +1,60 @@
+// 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"
+ 
+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)
+{
+   using namespace Eigen;
+   assert (X.rows() == Y.rows() && "Same number of points");
+   assert(X.cols() == Y.cols() && "Points have same dimensions");
+
+   // Center data
+   VectorXd Xmean = X.colwise().mean();      
+   VectorXd Ymean = Y.colwise().mean();      
+   MatrixXd XC = X.rowwise() - Xmean.transpose();
+   MatrixXd YC = Y.rowwise() - Ymean.transpose();
+
+
+   // Determine scale
+   double 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 M = XC.transpose() * YC; 
+   JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+   MatrixXd sigma;
+   sigma.setIdentity(DIM,DIM);
+   if (!includeReflections && (svd.matrixU() * svd.matrixV().transpose()).determinant() < 0)
+       sigma(DIM-1,DIM-1) = -1.;
+
+   Matrix<Scalar,DIM,DIM> R = svd.matrixU() * sigma * svd.matrixV().transpose();
+   assert(abs(R.determinant() - 1) < 1e-10);
+
+
+   // Translation
+   Matrix<Scalar,DIM,1> t = Ymean - scale*R*Xmean;
+     
+
+   // Combine
+   T = Translation<Scalar,DIM>(t) * R.transpose() * Scaling(scale);
+}

include/igl/procrustes.h

@@ -0,0 +1,53 @@
+// 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, rotation/reflection R  and translation t 
+    // s.t. |s*X*R + t - Y|^2 is minimized.
+    //
+    // 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);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+    #include "procrustes.cpp"
+#endif
+
+#endif