147 lines
5.3 KiB
C++
147 lines
5.3 KiB
C++
/****************************************************************************
|
|
* VCGLib o o *
|
|
* Visual and Computer Graphics Library o o *
|
|
* _ O _ *
|
|
* Copyright(C) 2004 \/)\/ *
|
|
* 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 <eigenlib/Eigen/Core>
|
|
#include <eigenlib/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
|