diff --git a/vcg/complex/algorithms/point_matching_scale.h b/vcg/complex/algorithms/point_matching_scale.h new file mode 100644 index 00000000..d1dfb34c --- /dev/null +++ b/vcg/complex/algorithms/point_matching_scale.h @@ -0,0 +1,134 @@ +/**************************************************************************** +* 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. * +* * +****************************************************************************/ + +#include +#include +#include +#include + +namespace vcg { + +template +struct RotoTranslation +{ + RotoTranslation(){} + Scalar _v[6]; + void ToMatrix(vcg::Matrix44 & m) + { + vcg::Matrix44 rot,tra; + rot.FromEulerAngles(_v[0],_v[1],_v[2]); + tra.SetTranslate(vcg::Point3(_v[3],_v[4],_v[5])); + m = tra * rot; + } +}; + +class PointMatchingScale { +private: + static std::vector *fix; + static std::vector *mov; + static vcg::Box3d b; + +public: + /** + * Compute a scaling transformation that bring PMov point as close as possible to Pfix + */ + static void ComputeScalingMatchMatrix( + vcg::Matrix44d &res, + std::vector &Pfix, + std::vector &Pmov) + { + fix = &Pfix; + mov = &Pmov; + b.SetNull(); + for(std::vector::iterator i = Pmov.begin(); i != Pmov.end(); ++i) + b.Add(*i); + + double scale = 1.0; + min_newuoa(1,&scale,errorScale); + + res.SetTranslate( b.Center()*(1.0-scale)); + res[0][0] = res[1][1] = res[2][2] = scale; + } + + /** + * Compute a rototranslation + scaling transformation that bring PMov point as close as possible to Pfix + */ + static void ComputeRotoTranslationScalingMatchMatrix( + vcg::Matrix44d &res, + std::vector &Pfix, + std::vector &Pmov) + { + fix = &Pfix; + mov = &Pmov; + b.SetNull(); + for(std::vector::iterator i = Pmov.begin(); i != Pmov.end(); ++i) + b.Add(*i); + + double x[7]={1.0,0.0,0.0,0.0,0.0,0.0,0.0}; + min_newuoa(7,&x[0],errorRotoTranslationScale); + + // rtm = rototranslation + RotoTranslation rt; + vcg::Matrix44d rtm; + memcpy(&rt._v[0],&x[1],6*sizeof(double)); + rt.ToMatrix(rtm); + + // res= scaling w.r.t. barycenter + res.SetTranslate( b.Center()*(1.0-x[0])); + res[0][0] = res[1][1] = res[2][2] = x[0]; + res = rtm*res; + } + + static double errorScale(int n, double *x) + { + assert(n==1); (void)n; + double dist = 0; + std::vector::iterator i = mov->begin(); + std::vector::iterator ifix = fix->begin(); + for(; i != mov->end(); ++i,++ifix) + dist += vcg::SquaredDistance(((*i)-b.Center())*(*x)+b.Center() , *ifix); + + return dist; + } + + static double errorRotoTranslationScale(int n, double *x) { + assert(n==7); (void)n; + double dist = 0; + std::vector::iterator i = mov->begin(); + std::vector::iterator ifix = fix->begin(); + + RotoTranslation rt; + vcg::Matrix44d m; + memcpy(&rt._v[0],&x[1],6*sizeof(double)); + rt.ToMatrix(m); + + for(; i != mov->end(); ++i,++ifix) { + dist += vcg::SquaredDistance( m*(((*i)-b.Center())*(x[0])+b.Center()),*ifix); + } + return dist; + } + +}; + +} +