vcglib/vcg/space/fitting3.h

147 lines
5.2 KiB
C
Raw Normal View History

2005-03-14 18:04:24 +01:00
/****************************************************************************
* 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>
2005-10-13 16:59:57 +02:00
#include <vcg/math/matrix44.h>
#include <vcg/math/matrix33.h>
2005-03-14 18:04:24 +01:00
#include <eigenlib/Eigen/Core>
#include <eigenlib/Eigen/Eigenvalues>
2005-03-14 18:04:24 +01:00
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::Vector3f 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::Matrix3f covMat=Eigen::Matrix3f::Zero();
Point3<S> b;
ComputeCovarianceMatrix(pointVec,b,covMat);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(covMat);
Eigen::Vector3f eval = eig.eigenvalues();
Eigen::Matrix3f 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);
2005-10-13 16:59:57 +02:00
}
/*! \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)
2005-03-14 18:04:24 +01:00
{
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::Vector3f 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;
2005-03-14 18:04:24 +01:00
}
/*! \brief Compute the plane best fitting a set of points
2005-03-14 18:04:24 +01:00
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)
2005-03-14 18:04:24 +01:00
{
Eigen::Matrix3f covMat=Eigen::Matrix3f::Zero();
Point3<S> b;
ComputeWeightedCovarianceMatrix(pointVec,weightVec, b,covMat);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(covMat);
Eigen::Vector3f eval = eig.eigenvalues();
Eigen::Matrix3f 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);
2005-03-14 18:04:24 +01:00
}
2005-03-14 18:04:24 +01:00
} // end namespace
2005-10-15 14:58:13 +02:00
#endif