add vcglib

vcglib/vcg/space/fitting3.h

@@ -0,0 +1,146 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+#ifndef __VCGLIB_FITTING3
+#define __VCGLIB_FITTING3
+
+#include <vector>
+#include <vcg/space/plane3.h>
+#include <vcg/math/matrix44.h>
+#include <vcg/math/matrix33.h>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+
+namespace vcg {
+
+/*! \brief compute the covariance matrix of a set of point
+
+  It returns also the barycenter of the point set
+  */
+template <class S >
+void ComputeCovarianceMatrix(const std::vector<Point3<S> > &pointVec, Point3<S> &barycenter, Eigen::Matrix<S,3,3> &m)
+{
+  // first cycle: compute the barycenter
+  barycenter.SetZero();
+  typename  std::vector< Point3<S> >::const_iterator pit;
+  for( pit = pointVec.begin(); pit != pointVec.end(); ++pit) barycenter+= (*pit);
+  barycenter/=pointVec.size();
+
+  // second cycle: compute the covariance matrix
+  m.setZero();
+  Eigen::Matrix<S,3,1> p;
+  for(pit = pointVec.begin(); pit != pointVec.end(); ++pit) {
+    ((*pit)-barycenter).ToEigenVector(p);
+    m+= p*p.transpose(); // outer product
+  }
+}
+
+/*! \brief Compute the plane best fitting a set of points
+
+The algorithm used is the classical Covariance matrix eigenvector approach.
+
+*/
+template <class S>
+void FitPlaneToPointSet(const std::vector< Point3<S> > & pointVec, Plane3<S> & plane)
+{
+  Eigen::Matrix<S,3,3> covMat = Eigen::Matrix<S,3,3>::Zero();
+  Point3<S> b;
+  ComputeCovarianceMatrix(pointVec,b,covMat);
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<S,3,3> > eig(covMat);
+  Eigen::Matrix<S,3,1> eval = eig.eigenvalues();
+  Eigen::Matrix<S,3,3> evec = eig.eigenvectors();
+  eval = eval.cwiseAbs();
+  int minInd;
+  eval.minCoeff(&minInd);
+  Point3<S> d;
+  d[0] = evec(0,minInd);
+  d[1] = evec(1,minInd);
+  d[2] = evec(2,minInd);
+
+  plane.Init(b,d);
+}
+
+/*! \brief compute the weighted covariance matrix of a set of point
+
+  It returns also the weighted barycenter of the point set.
+  When computing the covariance matrix  the weights are applied to the points transposed to the origin.
+  */
+template <class S >
+void ComputeWeightedCovarianceMatrix(const std::vector<Point3<S> > &pointVec, const std::vector<S> &weightVec, Point3<S> &bp, Eigen::Matrix<S,3,3> &m)
+{
+  assert(pointVec.size() == weightVec.size());
+  // First cycle: compute the weighted barycenter
+  bp.SetZero();
+  S wSum=0;
+  typename  std::vector< Point3<S> >::const_iterator pit;
+  typename  std::vector<  S>::const_iterator wit;
+  for( pit = pointVec.begin(),wit=weightVec.begin(); pit != pointVec.end(); ++pit,++wit)
+  {
+    bp+= (*pit)*(*wit);
+    wSum+=*wit;
+  }
+  bp /=wSum;
+
+  // Second cycle: compute the weighted covariance matrix
+  // The weights are applied to the points transposed to the origin.
+  m.setZero();
+  Eigen::Matrix<S,3,3> A;
+  Eigen::Matrix<S,3,1> p;
+  for( pit = pointVec.begin(),wit=weightVec.begin(); pit != pointVec.end(); ++pit,++wit)
+  {
+    (((*pit)-bp)*(*wit)).ToEigenVector(p);
+    m+= p*p.transpose(); // outer product
+  }
+  m/=wSum;
+}
+/*! \brief Compute the plane best fitting a set of points
+
+The algorithm used is the classical Covariance matrix eigenvector approach.
+*/
+
+template <class S>
+void WeightedFitPlaneToPointSet(const std::vector< Point3<S> > & pointVec, const std::vector<S> &weightVec, Plane3<S> & plane)
+{
+  Eigen::Matrix<S,3,3> covMat = Eigen::Matrix<S,3,3>::Zero();
+  Point3<S> b;
+  ComputeWeightedCovarianceMatrix(pointVec,weightVec, b,covMat);
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<S,3,3> > eig(covMat);
+  Eigen::Matrix<S,3,1> eval = eig.eigenvalues();
+  Eigen::Matrix<S,3,3> evec = eig.eigenvectors();
+  eval = eval.cwiseAbs();
+  int minInd;
+  eval.minCoeff(&minInd);
+  Point3<S> d;
+  d[0] = evec(0,minInd);
+  d[1] = evec(1,minInd);
+  d[2] = evec(2,minInd);
+
+  plane.Init(b,d);
+}
+
+} // end namespace
+
+#endif