vcglib/vcg/space/fitting3.h

147 lines
5.3 KiB
C++

/****************************************************************************
* 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