Rewrote of the Fitting to plane functions. Added Weighted version and sample
This commit is contained in:
parent
3a6031554b
commit
c87cd495de
|
|
@ -9,6 +9,7 @@ SUBDIRS = trimesh_base \
|
|||
trimesh_curvature \
|
||||
trimesh_clustering \
|
||||
trimesh_edge \
|
||||
trimesh_fitting \
|
||||
# trimesh_ext_mc \
|
||||
trimesh_hole \
|
||||
trimesh_inertia \
|
||||
|
|
|
|||
|
|
@ -0,0 +1,111 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004-2012 \/)\/ *
|
||||
* 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. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/*! \file trimesh_fitting.cpp
|
||||
\ingroup code_sample
|
||||
|
||||
\brief A small example about sampling and fitting
|
||||
|
||||
Given a mesh (an icosahedron) for each face we get a few random samples over it, and then we recover:
|
||||
- the plane fitting them (that coincide with the face plane and exactly approximate all the sample points)
|
||||
- the plane fitting the perturbed version of this set
|
||||
- the plane fitting the perturbed version of the set but using a weighted fitting scheme.
|
||||
*/
|
||||
|
||||
#include<vcg/complex/complex.h>
|
||||
#include<vcg/complex/algorithms/update/topology.h>
|
||||
#include<vcg/complex/algorithms/update/normal.h>
|
||||
#include<vcg/complex/algorithms/create/platonic.h>
|
||||
#include<vcg/complex/algorithms/point_sampling.h>
|
||||
#include<wrap/io_trimesh/import_off.h>
|
||||
#include<vcg/space/fitting3.h>
|
||||
|
||||
|
||||
class MyEdge;
|
||||
class MyFace;
|
||||
class MyVertex;
|
||||
struct MyUsedTypes : public vcg::UsedTypes< vcg::Use<MyVertex> ::AsVertexType,
|
||||
vcg::Use<MyEdge> ::AsEdgeType,
|
||||
vcg::Use<MyFace> ::AsFaceType>{};
|
||||
|
||||
class MyVertex : public vcg::Vertex<MyUsedTypes, vcg::vertex::Coord3f, vcg::vertex::Normal3f, vcg::vertex::BitFlags >{};
|
||||
class MyFace : public vcg::Face< MyUsedTypes, vcg::face::FFAdj, vcg::face::VertexRef, vcg::face::Normal3f, vcg::face::BitFlags > {};
|
||||
class MyEdge : public vcg::Edge<MyUsedTypes>{};
|
||||
class MyMesh : public vcg::tri::TriMesh< std::vector<MyVertex>, std::vector<MyFace> , std::vector<MyEdge> > {};
|
||||
|
||||
float EvalPlane(vcg::Plane3f &pl, vcg::Point3f &dir, std::vector<vcg::Point3f> posVec)
|
||||
{
|
||||
float off=0;
|
||||
for(int i=0;i<posVec.size();++i)
|
||||
off += fabs(vcg::SignedDistancePlanePoint(pl,posVec[i]));
|
||||
|
||||
off/=float(posVec.size());
|
||||
return off;
|
||||
}
|
||||
|
||||
|
||||
int main( int argc, char **argv )
|
||||
{
|
||||
MyMesh m;
|
||||
vcg::tri::Icosahedron(m);
|
||||
vcg::tri::UpdateNormal<MyMesh>::PerVertexNormalizedPerFaceNormalized(m);
|
||||
vcg::tri::UpdateBounding<MyMesh>::Box(m);
|
||||
|
||||
// As a simple test just run over all the faces of a mesh
|
||||
// get a few random points over it, perturb them and fit a plane on them.
|
||||
vcg::Plane3f ple,plf,plw;
|
||||
int cnt=0;
|
||||
float scaleFac = m.bbox.Diag()/10.0f;
|
||||
printf("ScaleFac %f\n\n",scaleFac);
|
||||
for(int i=0;i<m.FN();++i)
|
||||
{
|
||||
std::vector<vcg::Point3f> ExactVec;
|
||||
std::vector<vcg::Point3f> PerturbVec;
|
||||
std::vector<float> WeightVec;
|
||||
vcg::Plane3f pl;
|
||||
pl.Init(vcg::Barycenter(m.face[i]),m.face[i].N());
|
||||
for(int j=0;j<200;++j)
|
||||
{
|
||||
vcg::Point3f p = vcg::tri::SurfaceSampling<MyMesh>::RandomPointInTriangle(m.face[i]);
|
||||
ExactVec.push_back(p);
|
||||
vcg::Point3f off=vcg::tri::SurfaceSampling<MyMesh>::RandomPoint3fBall01();
|
||||
p+=off*scaleFac;
|
||||
float w = std::max(0.0, 1.0f-fabs(vcg::SignedDistancePlanePoint(pl,p))/scaleFac);
|
||||
PerturbVec.push_back(p);
|
||||
WeightVec.push_back(w*w); // as weight we use the square of (1-distance)
|
||||
}
|
||||
|
||||
vcg::FitPlaneToPointSet(ExactVec,ple);
|
||||
float err=EvalPlane(ple,m.face[i].N(),ExactVec);
|
||||
|
||||
vcg::FitPlaneToPointSet(PerturbVec,plf);
|
||||
float err0=EvalPlane(plf,m.face[i].N(),ExactVec);
|
||||
|
||||
vcg::WeightedFitPlaneToPointSet(PerturbVec,WeightVec,plw);
|
||||
float err1=EvalPlane(plw,m.face[i].N(),ExactVec);
|
||||
printf("Exact %5.3f Fit to Perturbed %5.3f Weighted fit to perturbed %5.3f\n",err,err0,err1);
|
||||
if(err0>err1) cnt++;
|
||||
}
|
||||
|
||||
printf("\nWeighted Fitting was better %i on %i\n",cnt,m.FN());
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
include(../common.pri)
|
||||
TARGET = trimesh_fitting
|
||||
SOURCES += trimesh_fitting.cpp
|
||||
|
|
@ -19,18 +19,6 @@
|
|||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
History
|
||||
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.2 2005/10/13 14:59:57 ganovelli
|
||||
versione con svd
|
||||
|
||||
Revision 1.1 2005/03/14 17:04:24 ganovelli
|
||||
created
|
||||
|
||||
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __VCGLIB_FITTING3
|
||||
|
|
@ -40,166 +28,120 @@ created
|
|||
#include <vcg/space/plane3.h>
|
||||
#include <vcg/math/matrix44.h>
|
||||
#include <vcg/math/matrix33.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
|
||||
#include <eigenlib/Eigen/Core>
|
||||
#include <eigenlib/Eigen/Eigenvalues>
|
||||
|
||||
namespace vcg {
|
||||
|
||||
template <class S>
|
||||
Point3<S> PlaneFittingPoints(const std::vector< Point3<S> > & samples, Plane3<S> & p, Point4<S> & eval, Matrix44<S> & evec)
|
||||
{
|
||||
Matrix44<S> m;m.SetZero();
|
||||
typename std::vector< Point3<S> >::const_iterator it;
|
||||
/*! \brief compute the covariance matrix of a set of point
|
||||
|
||||
Point3<S> c; c.SetZero();
|
||||
for(it = samples.begin(); it != samples.end(); ++it)
|
||||
{
|
||||
c += *it;
|
||||
}
|
||||
c /= samples.size();
|
||||
|
||||
for(it = samples.begin(); it != samples.end(); ++it)
|
||||
{
|
||||
Point3<S> p = (*it) - c;
|
||||
for(int j = 0 ; j < 3;++j)
|
||||
*(Point3<S>*)&m[j][0] += p * p[j];
|
||||
}
|
||||
|
||||
m[0][3] = m[1][3] = m[2][3] = S(0);
|
||||
m[3][3] = S(1);
|
||||
m[3][0] = m[3][1] = m[3][2] = S(0);
|
||||
|
||||
int n;
|
||||
Point3<S> d;
|
||||
Jacobi(m, eval, evec, n);
|
||||
|
||||
//Sort eigenvalues (tarinisort)
|
||||
Point4<S> e;
|
||||
e[0] = S(fabs(eval[0]));
|
||||
e[1] = S(fabs(eval[1]));
|
||||
e[2] = S(fabs(eval[2]));
|
||||
|
||||
int maxi, mini, medi;
|
||||
if (e[1] > e[0]) { maxi = 1; mini = 0; } else { maxi = 0; mini = 1;}
|
||||
if (e[maxi] < e[2]) { maxi = 2; } else if (e[mini] > e[2]) { mini = 2; };
|
||||
medi = 3 - maxi -mini;
|
||||
|
||||
d[0] = evec[0][mini];
|
||||
d[1] = evec[1][mini];
|
||||
d[2] = evec[2][mini];
|
||||
|
||||
const S norm = d.Norm();
|
||||
p.SetOffset(c.dot(d)/norm);
|
||||
p.SetDirection(d/norm);
|
||||
|
||||
return Point3<S>(e[mini], e[medi], e[maxi]);
|
||||
}
|
||||
|
||||
template <class S>
|
||||
Point3<S> PlaneFittingPoints(const std::vector< Point3<S> > & samples, Plane3<S> & p)
|
||||
{
|
||||
Point4<S> eval;
|
||||
Matrix44<S> evec;
|
||||
return PlaneFittingPoints(samples, p, eval, evec);
|
||||
}
|
||||
|
||||
template<class S>
|
||||
inline double FIT_VExp( const Point3<S> & x, const int i )
|
||||
{
|
||||
assert(i>=0);
|
||||
assert(i<4);
|
||||
if(i==0) return 1;
|
||||
else return x[i-1];
|
||||
}
|
||||
|
||||
/** Fitting di piani: trova il piano che meglio approssima
|
||||
l'insieme di punti dato
|
||||
It returns also the barycenter of the point set
|
||||
*/
|
||||
template<class S>
|
||||
bool PlaneFittingPointsOld( std::vector< Point3<S> > & samples, Plane3<S> & p )
|
||||
template <class S >
|
||||
void ComputeCovarianceMatrix(const std::vector<Point3<S> > &pointVec, Point3<S> &barycenter, Eigen::Matrix<S,3,3> &m)
|
||||
{
|
||||
Point3<S> d;
|
||||
// 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();
|
||||
|
||||
const int N = 4;
|
||||
S P[N][N]; // A = s' . s
|
||||
S U[N][N];
|
||||
int i,j,k,n;
|
||||
|
||||
n = (int)samples.size();
|
||||
if(n<3)
|
||||
return false;
|
||||
|
||||
//printf("\n p_prima: %f %f %f %f \n",p.Offset(),p.Direction()[0],p.Direction()[1],p.Direction()[2]);
|
||||
|
||||
for(i=0;i<N;++i)
|
||||
{
|
||||
for(j=i;j<N;++j)
|
||||
{
|
||||
P[i][j] = 0;
|
||||
for(k=0;k<n;++k)
|
||||
P[i][j] += FIT_VExp(samples[k],i) * FIT_VExp(samples[k],j);
|
||||
// second cycle: compute the covariance matrix
|
||||
m.setZero();
|
||||
Eigen::Matrix<S,3,3> A;
|
||||
Eigen::Vector3f p;
|
||||
for(pit = pointVec.begin(); pit != pointVec.end(); ++pit) {
|
||||
((*pit)-barycenter).ToEigenVector(p);
|
||||
m+= p*p.transpose(); // outer product
|
||||
}
|
||||
for(j=0;j<i;++j)
|
||||
P[i][j] = P[j][i];
|
||||
}
|
||||
|
||||
//printf("D \n");
|
||||
//for(i=0;i<N;++i){
|
||||
// printf("\n");
|
||||
// for(j=0;j<N;++j)
|
||||
// printf("%2.3f\t",P[i][j]);
|
||||
//}
|
||||
//
|
||||
Matrix44<S> m;
|
||||
for(i=0;i<N;++i)
|
||||
for(j=0;j<N;++j)
|
||||
m[i][j]=P[i][j];
|
||||
|
||||
|
||||
// Point4<S> s;s.SetZero();
|
||||
//
|
||||
// s.Normalize();
|
||||
// printf("\n RES %f %f %f %f \n",s[0],s[1],s[2],s[3]);
|
||||
//printf("\n GJ \n");
|
||||
// for(i=0;i<N;++i){
|
||||
// printf("\n");
|
||||
// for(j=0;j<N;++j)
|
||||
// printf("%2.3f\t",m[i][j]);
|
||||
// }
|
||||
for(i=0;i<N;++i)
|
||||
{
|
||||
U[i][i] = 1.0;
|
||||
for(j=0;j<i;++j)
|
||||
U[i][j] = 0.0;
|
||||
for(j=i+1;j<N;++j)
|
||||
{
|
||||
if(P[i][i]==0.0)
|
||||
return false;
|
||||
|
||||
U[i][j] = P[i][j]/P[i][i];
|
||||
for(k=j;k<N;++k)
|
||||
P[j][k] -= U[i][j]*P[i][k];
|
||||
}
|
||||
}
|
||||
|
||||
//printf("\n U \n");
|
||||
//for(i=0;i<N;++i){
|
||||
// printf("\n");
|
||||
// for(j=0;j<N;++j)
|
||||
// printf("%2.3f\t",U[i][j]);
|
||||
//}
|
||||
|
||||
|
||||
S norm = Point3<S>(U[1][2]*U[2][3]-U[1][3],-U[2][3],1).Norm();
|
||||
|
||||
p.SetDirection(Point3<S>(U[1][2]*U[2][3]-U[1][3],-U[2][3],1));
|
||||
p.SetOffset(-(U[0][2]*U[2][3]-U[0][3]+U[0][1]*U[1][3]-U[0][1]*U[1][2]*U[2][3])/norm);
|
||||
|
||||
|
||||
//printf("\n p: %f %f %f %f \n",p.Offset(),p.Direction()[0],p.Direction()[1],p.Direction()[2]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*! \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);
|
||||
}
|
||||
|
||||
/*! \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::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;
|
||||
}
|
||||
/*! \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::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);
|
||||
}
|
||||
|
||||
} // end namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue