From c87cd495dec3b93c5c746872075affad8bd3fb13 Mon Sep 17 00:00:00 2001 From: cignoni Date: Mon, 22 Oct 2012 18:27:30 +0000 Subject: [PATCH] Rewrote of the Fitting to plane functions. Added Weighted version and sample --- apps/sample/sample.pro | 1 + .../trimesh_fitting/trimesh_fitting.cpp | 111 ++++++++ .../trimesh_fitting/trimesh_fitting.pro | 3 + vcg/space/fitting3.h | 262 +++++++----------- 4 files changed, 217 insertions(+), 160 deletions(-) create mode 100644 apps/sample/trimesh_fitting/trimesh_fitting.cpp create mode 100644 apps/sample/trimesh_fitting/trimesh_fitting.pro diff --git a/apps/sample/sample.pro b/apps/sample/sample.pro index 0ac195a2..9eb0a9d4 100644 --- a/apps/sample/sample.pro +++ b/apps/sample/sample.pro @@ -9,6 +9,7 @@ SUBDIRS = trimesh_base \ trimesh_curvature \ trimesh_clustering \ trimesh_edge \ + trimesh_fitting \ # trimesh_ext_mc \ trimesh_hole \ trimesh_inertia \ diff --git a/apps/sample/trimesh_fitting/trimesh_fitting.cpp b/apps/sample/trimesh_fitting/trimesh_fitting.cpp new file mode 100644 index 00000000..6cb96d91 --- /dev/null +++ b/apps/sample/trimesh_fitting/trimesh_fitting.cpp @@ -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 +#include +#include +#include +#include +#include +#include + + +class MyEdge; +class MyFace; +class MyVertex; +struct MyUsedTypes : public vcg::UsedTypes< vcg::Use ::AsVertexType, + vcg::Use ::AsEdgeType, + vcg::Use ::AsFaceType>{}; + +class MyVertex : public vcg::Vertex{}; +class MyFace : public vcg::Face< MyUsedTypes, vcg::face::FFAdj, vcg::face::VertexRef, vcg::face::Normal3f, vcg::face::BitFlags > {}; +class MyEdge : public vcg::Edge{}; +class MyMesh : public vcg::tri::TriMesh< std::vector, std::vector , std::vector > {}; + +float EvalPlane(vcg::Plane3f &pl, vcg::Point3f &dir, std::vector posVec) +{ + float off=0; + for(int i=0;i::PerVertexNormalizedPerFaceNormalized(m); + vcg::tri::UpdateBounding::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 ExactVec; + std::vector PerturbVec; + std::vector 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::RandomPointInTriangle(m.face[i]); + ExactVec.push_back(p); + vcg::Point3f off=vcg::tri::SurfaceSampling::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; +} diff --git a/apps/sample/trimesh_fitting/trimesh_fitting.pro b/apps/sample/trimesh_fitting/trimesh_fitting.pro new file mode 100644 index 00000000..d8fabbb2 --- /dev/null +++ b/apps/sample/trimesh_fitting/trimesh_fitting.pro @@ -0,0 +1,3 @@ +include(../common.pri) +TARGET = trimesh_fitting +SOURCES += trimesh_fitting.cpp diff --git a/vcg/space/fitting3.h b/vcg/space/fitting3.h index 3110b372..1ad5410f 100644 --- a/vcg/space/fitting3.h +++ b/vcg/space/fitting3.h @@ -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 #include #include -#include +#include +#include namespace vcg { -template -Point3 PlaneFittingPoints(const std::vector< Point3 > & samples, Plane3 & p, Point4 & eval, Matrix44 & evec) +/*! \brief compute the covariance matrix of a set of point + + It returns also the barycenter of the point set + */ +template +void ComputeCovarianceMatrix(const std::vector > &pointVec, Point3 &barycenter, Eigen::Matrix &m) { - Matrix44 m;m.SetZero(); - typename std::vector< Point3 >::const_iterator it; + // first cycle: compute the barycenter + barycenter.SetZero(); + typename std::vector< Point3 >::const_iterator pit; + for( pit = pointVec.begin(); pit != pointVec.end(); ++pit) barycenter+= (*pit); + barycenter/=pointVec.size(); - Point3 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 p = (*it) - c; - for(int j = 0 ; j < 3;++j) - *(Point3*)&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 d; - Jacobi(m, eval, evec, n); - - //Sort eigenvalues (tarinisort) - Point4 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(e[mini], e[medi], e[maxi]); + // second cycle: compute the covariance matrix + m.setZero(); + Eigen::Matrix A; + 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 +void FitPlaneToPointSet(const std::vector< Point3 > & pointVec, Plane3 & plane) +{ + Eigen::Matrix3f covMat=Eigen::Matrix3f::Zero(); + Point3 b; + ComputeCovarianceMatrix(pointVec,b,covMat); + + Eigen::SelfAdjointEigenSolver eig(covMat); + Eigen::Vector3f eval = eig.eigenvalues(); + Eigen::Matrix3f evec = eig.eigenvectors(); + eval = eval.cwiseAbs(); + int minInd; + eval.minCoeff(&minInd); + Point3 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 +void ComputeWeightedCovarianceMatrix(const std::vector > &pointVec, const std::vector &weightVec, Point3 &bp, Eigen::Matrix &m) +{ + assert(pointVec.size() == weightVec.size()); + // First cycle: compute the weighted barycenter + bp.SetZero(); + S wSum=0; + typename std::vector< Point3 >::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 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 -Point3 PlaneFittingPoints(const std::vector< Point3 > & samples, Plane3 & p) +void WeightedFitPlaneToPointSet(const std::vector< Point3 > & pointVec, const std::vector &weightVec, Plane3 & plane) { - Point4 eval; - Matrix44 evec; - return PlaneFittingPoints(samples, p, eval, evec); + Eigen::Matrix3f covMat=Eigen::Matrix3f::Zero(); + Point3 b; + ComputeWeightedCovarianceMatrix(pointVec,weightVec, b,covMat); + + Eigen::SelfAdjointEigenSolver eig(covMat); + Eigen::Vector3f eval = eig.eigenvalues(); + Eigen::Matrix3f evec = eig.eigenvectors(); + eval = eval.cwiseAbs(); + int minInd; + eval.minCoeff(&minInd); + Point3 d; + d[0] = evec(0,minInd); + d[1] = evec(1,minInd); + d[2] = evec(2,minInd); + + plane.Init(b,d); } -template -inline double FIT_VExp( const Point3 & 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 - */ -template -bool PlaneFittingPointsOld( std::vector< Point3 > & samples, Plane3 & p ) -{ - Point3 d; - - 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 m; - for(i=0;i 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(U[1][2]*U[2][3]-U[1][3],-U[2][3],1).Norm(); - - p.SetDirection(Point3(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; -} } // end namespace #endif