1 /**************************************************************************** 2 * MeshLab o o * 3 * A versatile mesh processing toolbox o o * 4 * _ O _ * 5 * Copyright(C) 2007 \/)\/ * 6 * Visual Computing Lab /\/| * 7 * ISTI - Italian National Research Council | * 8 * \ * 9 * All rights reserved. * 10 * * 11 * This program is free software; you can redistribute it and/or modify * 12 * it under the terms of the GNU General Public License as published by * 13 * the Free Software Foundation; either version 2 of the License, or * 14 * (at your option) any later version. * 15 * * 16 * This program is distributed in the hope that it will be useful, * 17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * 20 * for more details. * 21 * * 22 ****************************************************************************/ 23 #ifndef FEATURE_ALIGNMENT_H 24 #define FEATURE_ALIGNMENT_H 25 26 #include <meshlab/meshmodel.h> 27 28 #include <vcg/container/simple_temporary_data.h> 29 #include <vcg/complex/algorithms/clustering.h> 30 31 #include <ANN/ANN.h> 32 #include <vcg/space/point_matching.h> 33 #include <vcg/space/index/grid_static_ptr.h> 34 #include <vcg/complex/algorithms/closest.h> 35 #include <vcg/complex/algorithms/point_sampling.h> 36 #include <vcg/complex/algorithms/overlap_estimation.h> 37 #include <meshlabplugins/edit_pickpoints/pickedPoints.h> 38 39 #include <qdatetime.h> 40 41 using namespace std; 42 using namespace vcg; 43 44 //needed stuffs to declare a mesh type ad hoc for clustering features. Used inside Matching(). 45 class MyEdge; 46 class MyFace; 47 class MyVertex : public VertexSimp2<MyVertex, MyEdge, MyFace, vertex::Coord3f, vertex::Normal3f, vertex::BitFlags > {}; 48 class MyFace: public vcg::FaceSimp2<MyVertex,MyEdge,MyFace, vcg::face::VertexRef>{}; 49 class MyMesh : public tri::TriMesh<vector<MyVertex>, vector<MyFace> >{}; 50 51 /** \brief This class provides a generic framework to automatically align two range maps or point clouds using 52 * a feature based approach. 53 * 54 * The framework select a base of \c nBase features from the mesh \c mMov and for each feature finds the \c k 55 * more similar features on the mesh \c mFix . Then the space of all the \c nBase tuples is searched with an 56 * efficient Branch & Bound algorithm and promising matching bases are keeped. This steps are repeated \c ransacIter 57 * times. At this point all the transformations are computed and tested. An alignment is considered good if the 58 * estimated overlap between meshes exceeds the \c consOffset*overlap %, otherways the alignment is discarded. 59 * The framework returns the best alignment found. 60 * 61 * The class is templated on the \c MESH_TYPE and on the \c FEATURE_TYPE . \c FEATURE_TYPE is a templated class 62 * that exposes a specific interface to compute and extract features. In this way the framework can abstract 63 * from features implementation's details and work with different kind of features. 64 * 65 * \par Important Note: 66 * For a safe use is very important that the init() function is called only once. If you need to change the input range maps 67 * you <em>must</em> destroy the old FeatureAlignment object and create a new one, then you can call the init() on the new 68 * object. Calling many times the init() on the same object will couse big memory leaks, becouse the init() allocates 69 * global structures but is the destructor that deallocates them. 70 */ 71 template<class MESH_TYPE, class FEATURE_TYPE> class FeatureAlignment 72 { 73 public: 74 typedef MESH_TYPE MeshType; 75 typedef FEATURE_TYPE FeatureType; 76 typedef typename MeshType::ScalarType ScalarType; 77 typedef typename MeshType::CoordType CoordType; 78 typedef typename MeshType::VertexType VertexType; 79 typedef typename MeshType::FaceType FaceType; 80 typedef Matrix44<ScalarType> Matrix44Type; 81 typedef typename MeshType::VertexIterator VertexIterator; 82 typedef OverlapEstimation<MeshType> ConsensusType; 83 typedef typename ConsensusType::Parameters ConsensusParam; 84 85 /// Different kind of sampling allowed on features. 86 enum SamplingStrategies { UNIFORM_SAMPLING=0, POISSON_SAMPLING=1 }; 87 88 private: 89 /// Inner class needed to perform sampling on pointers to vertexes. 90 class VertexPointerSampler 91 { 92 public: 93 94 MeshType* m; //this is needed for advanced sampling (i.e poisson sampling) 95 VertexPointerSampler()96 VertexPointerSampler(){ m = new MeshType(); m->Tr.SetIdentity(); m->sfn=0; } ~VertexPointerSampler()97 ~VertexPointerSampler(){ if(m) delete m; } 98 vector<VertexType*> sampleVec; 99 AddVert(VertexType & p)100 void AddVert(VertexType &p){ sampleVec.push_back(&p); } 101 AddFace(const FaceType & f,const CoordType & p)102 void AddFace(const FaceType &f, const CoordType &p){} 103 AddTextureSample(const FaceType &,const CoordType &,const Point2i &)104 void AddTextureSample(const FaceType &, const CoordType &, const Point2i &){} 105 }; 106 107 public: 108 /// Inner class to hold framework's parameters; this avoids endless parameters' list inside functions. 109 class Parameters{ 110 public: 111 112 int samplingStrategy; ///< Strategy to extract features from range maps. \c UNIFORM_SAMPLING is the default; \c POISSON_SAMPLING is highly time consuming and doesn't give best results. 113 int numFixFeatureSelected; ///< Number of feature sampled on \c mFix to match base. As default all the features are matched. 114 int numMovFeatureSelected; ///< Number of feature sampled on \c mMov to choose a base. Default is \c 250. 115 int ransacIter; ///< Number of iterations. More iterations means higher success probability, but of course it requires more time. 116 int nBase; ///< Number of features of a base. It should be at least \c 3, the default is \c 4, higher values doesn't seem to give higher success probability nor best quality in alignment. 117 int k; ///< Number of features picked by kNN search. Higher values should provide higher success probability, especially in case of mesh with many vertexes and/or low discriminant features. The drawback is a critical growth of computation time. Default is \c 75 . 118 int fullConsensusSamples; ///< Number of samples used to perform full consensus. Default is \c 2500 . 119 int short_cons_samples; ///< Number of samples used to perform short consensus. Default is \c 200 . 120 float consensusDist; ///< Consensus distance expressed as percentage of the BBox diagonal of \c mMov . Higher values should weaken the consensus test allowing false positive alignment. Deafault is \c 2% . 121 float consensusNormalsAngle; ///< Holds the the consensus angle for normals, in radians. This parameter is strictly related to \c consensusDist ; to weaken the consensus test both values should be increased. Default is \c 0.965 Rad, about \c 15 Deg. 122 float sparseBaseDist; ///< Distance used to select sparse bases from \c mMov . Distance is espressed as a percentage of BBox diagonal of \c mMov . Default is \c 2*consensusDist . 123 float mutualErrDist; ///< Distance used while exploring the matching bases solutions' space to establish if a matching base is admissible or not. Distance is espressed as a percentage of BBox diagonal of \c mMov . Default is \c consensusDist . 124 float overlap; ///< Overlap percentage; it measure how much \c mMov overlaps with \c mFix . Is very important provide a pecise estimation 'couse too low values can give in false positive alignments, instead too high values can turn down critically the success probability. Default is \c 75 , but user should always provide an appropriate value. 125 float succOffset; ///< Consensus percentage to early interrupt processing. The alignment found is considered good enough to stop the process. Percentage is related to \c overlap , i.e \c succOffset=100 is equal to \c overlap. Default is \c 90 . 126 float consOffset; ///< Consensus percentage to overcome to win consensus test. Percentage is related to \c overlap , i.e \c consOffset=100 is equal to \c overlap. Default is \c 90 . 127 bool normalEqualization; ///< If \c true , normal equalization sampling is used during consensus test, otherways uniform sampling is used. \c true is the default. 128 bool pickPoints; ///< Utility that stores a base and its match into picked points attribute. If \c true points are stored and they can be displayed later. 129 bool paint; ///< Utility to paint \c mMov according to consensus. If \c true vertexes of \c mMov are painted accordingly to the best consensus found. To get a detailed description of how the mesh is colored see vcg/complex/algorithms/overlap_estimation.h. 130 int maxNumFullConsensus; ///< Indicates the max number of bases tested with full consensus procedure. Defailt is \c 15. 131 int maxNumShortConsensus; ///< Indicates the max number of bases tested with short consensus procedure. Default is \c 50. 132 float mMovBBoxDiag; ///< BBox diagonal of \c mMov. This is used to compute parameters such as \c consensusDist, \c sparseBaseDist etc. 133 void (*log)(int level,const char * f, ... );///< Pointer to log function. Used to print stat infos. 134 135 /// Default constructor. Set all the parameters to their default values. Parameters(MeshType & mFix,MeshType & mMov)136 Parameters(MeshType& mFix, MeshType& mMov){ 137 setDefault(mFix, mMov); 138 } 139 140 private: 141 /// Set parameters to the default values setDefault(MeshType & mFix,MeshType & mMov)142 void setDefault(MeshType& mFix, MeshType& mMov) 143 { 144 samplingStrategy = FeatureAlignment::UNIFORM_SAMPLING; 145 numFixFeatureSelected = mFix.VertexNumber(); 146 numMovFeatureSelected = 250; 147 ransacIter = 500; 148 nBase = 4; 149 k = 75; 150 fullConsensusSamples = 2500; 151 short_cons_samples = 200; 152 consensusDist = 2.0f; 153 consensusNormalsAngle = 0.965f; //15 degrees. 154 sparseBaseDist = 2*consensusDist; 155 mutualErrDist = consensusDist; 156 overlap = 75.0f; 157 succOffset = 90.0f; 158 consOffset = 60.0f; 159 normalEqualization = true; 160 pickPoints = false; 161 paint = false; 162 maxNumFullConsensus = 15; 163 maxNumShortConsensus = 50; 164 mMovBBoxDiag = mMov.bbox.Diag(); 165 log = NULL; 166 } 167 }; 168 169 /// Class to hold the result of the alignment process. It holds many infos about stats and errors too. 170 class Result{ 171 public: 172 173 /// Exit codes of the alignment procedure. \arg \c ALIGNED Framework has found a good alignment. \arg \c NOT_ALIGNED Framework doesn't find a good alignment. \arg \c FAILED Something wrong happened during alignment process. 174 enum {ALIGNED, NOT_ALIGNED, FAILED}; 175 176 int exitCode; ///< Holds the exit code of the consensus procedure. It can be tested to know if the process has been completed successfully. 177 int errorCode; ///< It holds an integer code related to an error occured during the process. If no errors occured it should hold \c -1 , a positive integer otherways. A description of the error should be stored in \c errorMsg . 178 Matrix44Type tr; ///< It holds the transformation matrix that aligns the two range maps. If alignment has not been found it holds the identity matrix. 179 float bestConsensus; ///< It holds the estimated overlap for the returned alignment. Overlap is espressed in percentage relative to \c mMov . 180 int numBasesFound; ///< The number of bases selected on \c mMov . This number is limited by the number of iterations done, i.e \c ransacIter . 181 int numWonFullCons; ///< The number of consensus test exceeded. 182 int numMatches; ///< The number of admissible matching bases found after Branch&Bound procedure. 183 int totalTime; ///< The execution time of the the whole alignment process in milliseconds. 184 int baseSelectionTime; ///< The time spent to select bases from \c mMov in milliseconds. 185 int matchingTime; ///< The time spent to perform matching in milliseconds. 186 int branchBoundTime; ///< The time spent to perform Branch&Bound in milliseconds. 187 int rankTime; ///< The time spent to rank matching bases before consensus test in milliseconds. 188 int shortConsTime; ///< The time spent to perform short consensus test on \c maxNumShortConsensus matching bases in milliseconds. 189 int fullConsTime; ///< The time spent to perform short consensus test on \c maxNumFullConsensus matching bases in milliseconds. 190 int initTime; ///< The time spent to initialize structures in milliseconds. 191 QString errorMsg; ///< It holds a string describing what is gone wrong. It does make sense only if \c exitCode==FAILED . 192 193 ///Default connstructor. Initialize values. Result()194 Result(){ 195 exitCode = NOT_ALIGNED; 196 errorCode = -1; 197 tr = Matrix44Type::Identity(); 198 bestConsensus = 0.0f; 199 numBasesFound = 0; 200 numWonFullCons = 0; 201 numMatches = 0; 202 totalTime = 0; 203 initTime = 0; 204 matchingTime = 0; 205 shortConsTime = 0; 206 fullConsTime = 0; 207 baseSelectionTime = 0; 208 branchBoundTime = 0; 209 rankTime = 0; 210 errorMsg = "An unkown error occurred."; 211 } 212 }; 213 214 /// A type to hold useful infos about candidates. A candidate is an admissible matching base. It also provides functions to rank candidates properly. 215 struct CandidateType 216 { 217 int shortCons; ///< Short consensus score. Used to rank candidates. 218 float summedPointDist; ///< Sum of the distances betweeneach base point and its matching point, weighted with angle between normals. This is used to get a first ranking of the candidates. 219 Matrix44<ScalarType> tr; ///< Transformation matrix relative to this candidate, i.e th matrix the aligns the base on \c mMov to its matching base on \c mFix . 220 FeatureType** matchPtr; ///< A reference to the matching base on \c mFix . Features are not copied here but just referred through an array of \c nBase pointers. 221 FeatureType** basePtr; ///< A reference to the base on \c mMov . Features are not copied here but just referred through an array of \c nBase pointers. 222 223 /// Default constructor. basePtrCandidateType224 CandidateType(FeatureType** base, FeatureType** match, int cons = -1, float spd = numeric_limits<float>::max()):basePtr(base),matchPtr(match),shortCons(cons),summedPointDist(spd){} 225 /// Static function used to sort the candidates according their \c summedPointDist in increasing order. SortByDistanceCandidateType226 static bool SortByDistance(CandidateType i,CandidateType j) { return (i.summedPointDist<j.summedPointDist); } 227 /// Static function used to sort the candidates according their \c shortCons in decreasing order. SortByScoreCandidateType228 static bool SortByScore(CandidateType i,CandidateType j) { return (i.shortCons>j.shortCons); } 229 }; 230 231 vector<FeatureType*>* vecFFix; ///< Vector that holds pointers to features extracted from \c mFix . 232 vector<FeatureType*>* vecFMov; ///< Vector that holds pointers to features extracted from \c mMov . 233 Result res; ///< Structure that holds result and stats. 234 ANNpointArray fdataPts; ///< \e ANN structure that holds descriptors from which \e kdTree is built. 235 ANNkd_tree* fkdTree; ///< \e ANN \e kdTree structure. 236 ConsensusType cons; ///< Object used to estimate overlap, i.e to perform consensus tests. 237 ConsensusParam consParam; ///< Structure that hold consensus' paramaters. 238 239 /// Default constructor. It sets pointers to \c NULL for safety and create a struct for result and stats. FeatureAlignment()240 FeatureAlignment():fkdTree(NULL),vecFFix(NULL),vecFMov(NULL),res(Result()){} 241 242 /// Class destructor. It deallocates structures that has been allocated by \c init() into dynamic memory. ~FeatureAlignment()243 ~FeatureAlignment(){ 244 //Cleaning extracted features 245 if(vecFFix) vecFFix->clear(); delete vecFFix; vecFFix = NULL; 246 if(vecFMov) vecFMov->clear(); delete vecFMov; vecFMov = NULL; 247 //Cleaning ANN structures 248 FeatureAlignment::CleanKDTree(fkdTree, fdataPts, NULL, NULL, NULL, true); 249 } 250 251 /** \brief This function initializes structures needed by the framework. 252 * 253 * First it computes features for both meshes (if they have not been previosly computed), then features 254 * are extracted from meshes and stored into proper vectors. After ANN's structures (such as \e kdTree etc.) 255 * and consensus' structures are initialized. 256 * @return A Result object, so it can be used to detect faliures and retrieve a proper error message. 257 */ 258 Result& init(MeshType& mFix, MeshType& mMov, Parameters& param, CallBackPos* cb=NULL) 259 { 260 if(cb) cb(0,"Initializing..."); 261 QTime timer, t; //timers 262 timer.start(); 263 264 //compute feature for mFix and mMov if they are not computed yet 265 typename FeatureType::Parameters p; 266 FeatureType::SetupParameters(p); 267 if(!FeatureType::HasBeenComputed(mFix)){ 268 t.start(); if(cb) cb(1,"Computing features..."); 269 if(!FeatureType::ComputeFeature(mFix, p)){ FeatureAlignment::setError(1,res); return res; } 270 if(param.log) param.log(3,"Features on mFix computed in %i msec.",t.elapsed()); 271 } 272 if(!FeatureType::HasBeenComputed(mMov)){ 273 t.start(); if(cb) cb(1,"Computing features..."); 274 if(!FeatureType::ComputeFeature(mMov, p)){ FeatureAlignment::setError(1,res); return res; } 275 if(param.log) param.log(3,"Features on mMov computed in %i msec.",t.elapsed()); 276 } 277 278 //initialize a static random generator used inside FeatureUniform() 279 tri::SurfaceSampling<MeshType, VertexPointerSampler>::SamplingRandomGenerator().initialize(time(NULL)); 280 281 //extract features 282 vecFFix = FeatureAlignment::extractFeatures(param.numFixFeatureSelected, mFix, FeatureAlignment::UNIFORM_SAMPLING); 283 vecFMov = FeatureAlignment::extractFeatures(param.numMovFeatureSelected, mMov, param.samplingStrategy); 284 assert(vecFFix); assert(vecFMov); 285 286 //copy descriptors of mFix into ANN structures, then build kdtree... 287 FeatureAlignment::SetupKDTreePoints(*vecFFix, &fdataPts, FeatureType::getFeatureDimension()); 288 fkdTree = new ANNkd_tree(fdataPts,vecFFix->size(),FeatureType::getFeatureDimension()); 289 assert(fkdTree); 290 291 //consensus structure initialization 292 cons.SetFix(mFix); 293 cons.SetMove(mMov); 294 consParam.normalEqualization = param.normalEqualization; 295 cons.Init(consParam); 296 297 res.initTime = timer.elapsed(); //stop the timer 298 299 return res; 300 } 301 302 /** \brief This is the function that performs the actual alignment process. 303 * 304 * Alignment process steps: 305 * <ul><li> Performs safety checks and variables initializations.</li><li> Select randomly (at most) \c ransacIter sparse bases from \c mMov .</li> 306 * <li> for each base: </li><ul><li> for each point in the base do a \e kNNSearch to find the \c k more similar feature (\e Matching)</li> 307 * <li> use <em>Branch And Bound</em> to explore the solution space of all the matching bases and put the admissible ones in a candidates' vector</li></ul> 308 * <li> for all candidates: </li><ul><li>compute the transformation matrix </li><li> compute the \c summedPointDist </li></ul> 309 * <li> Sort candidates by increasing \c summedPointDist. </li><li> Performs short consensus test on (at most) first \c maxNumShortConsensus. </li><li> Sort candidates by decreasing \c shortCons. </li> 310 * <li> Performs full consensus test on (at most) first \c maxNumFullConsensus. </li><li> Return data relative to the best candidate found. </li></ul> 311 * @return A Result object containing the transformation matrix and many others stats infos. 312 */ 313 Result& align(MeshType& mFix, MeshType& mMov, Parameters& param, CallBackPos *cb=NULL) 314 { 315 QTime tot_timer, timer; //timers 316 tot_timer.start(); 317 318 assert(vecFMov); assert(vecFFix); 319 320 if(param.nBase>int(vecFMov->size())){ setError(2,res); return res; } //not enough features to pick a base 321 if(param.k>int(vecFFix->size())){ setError(3, res); return res; } //not enough features to pick k neighboors 322 323 //normalize normals 324 assert(mFix.HasPerVertexNormal()); //mesh doesn't have per vertex normals!!! 325 assert(mMov.HasPerVertexNormal()); 326 tri::UpdateNormals<MeshType>::NormalizeVertex(mFix); 327 tri::UpdateNormals<MeshType>::NormalizeVertex(mMov); 328 329 //variables declaration 330 res.exitCode = Result::NOT_ALIGNED; 331 int bestConsensus = 0; //best consensus score 332 int cons_succ = int((param.consOffset*param.overlap/100.0f)*(param.fullConsensusSamples/100.0f)); //number of vertices to win consensus 333 int ransac_succ = int((param.succOffset*param.overlap/100.0f)*(param.fullConsensusSamples/100.0f)); //number of vertices to win RANSAC 334 int bestConsIdx = -1; //index of the best candidate, needed to store picked points 335 336 //set up params for consensus 337 consParam.consensusDist=param.consensusDist; 338 consParam.consensusNormalsAngle=param.consensusNormalsAngle; 339 consParam.paint = param.paint; 340 341 //auxiliary vectors needed during the process 342 vector<FeatureType**>* baseVec = new vector<FeatureType**>(); //vector to hold bases of feature 343 vector<CandidateType>* candidates = new vector<CandidateType>(); //vector to hold canidates 344 vector<vector<FeatureType*> > matches; //vector of vectors of features. Each vector contains all the feature matches for a base point. 345 346 //variables needed for progress bar callback 347 float progBar = 1.0f; 348 if(cb) cb(int(progBar),"Selecting bases..."); 349 350 //loop to select bases from mMov 351 timer.start(); 352 for(int i = 0; i<param.ransacIter; i++) 353 { 354 int errCode = FeatureAlignment::SelectBase(*vecFMov,*baseVec, param); 355 if(errCode) continue; //can't find a base, skip this iteration 356 } 357 res.numBasesFound=baseVec->size(); 358 res.baseSelectionTime = timer.elapsed(); 359 if(param.log) param.log(3,"%i bases found in %i msec.", baseVec->size(), res.baseSelectionTime); 360 361 //loop to find admissible candidates 362 for(int i = 0; i<baseVec->size(); i++) 363 { 364 timer.start(); 365 int errCode = FeatureAlignment::Matching(*vecFFix, *vecFMov, fkdTree, (*baseVec)[i], matches, param); 366 res.matchingTime+= timer.elapsed(); 367 368 timer.start(); 369 errCode = FeatureAlignment::BranchAndBound((*baseVec)[i], matches, *candidates, param); 370 matches.clear(); //make matches' vector ready for another iteration 371 res.branchBoundTime+= timer.elapsed(); 372 373 if(cb){ progBar+=(20.0f/baseVec->size()); cb(int(progBar),"Matching..."); } 374 } 375 res.numMatches = candidates->size(); 376 if(param.log) param.log(3,"%matching performed in %i msec.", res.matchingTime); 377 if(param.log) param.log(3,"%Branch&Bound performed in %i msec.", res.branchBoundTime); 378 if(param.log) param.log(3,"%i candidates found in %i msec.", candidates->size(), res.matchingTime+res.branchBoundTime); 379 380 //loop to compute candidates' transformation matrix and perform a first ranking using summedPointDist 381 timer.start(); 382 for(unsigned int j=0; j<candidates->size(); j++) 383 { 384 CandidateType& currCandidate = (*candidates)[j]; 385 386 //computes the rigid transformation matrix that overlaps the two points sets 387 Matrix44Type tr; 388 int errCode = FeatureAlignment::FindRigidTransformation(mFix, mMov, currCandidate.basePtr, currCandidate.matchPtr, param.nBase, tr); 389 if(errCode) continue; //can't find a rigid transformation, skip this iteration 390 391 currCandidate.tr = tr; //store transformation 392 393 Matrix44Type oldTr = ApplyTransformation(mMov, tr); //apply transformation 394 currCandidate.summedPointDist = SummedPointDistances(mFix, mMov, currCandidate.basePtr, currCandidate.matchPtr, param.nBase); //compute and store the sum of points distance 395 ResetTransformation(mMov, oldTr); //restore old tranformation 396 397 if(cb){ progBar+=(20.0f/candidates->size()); cb(int(progBar),"Ranking candidates..."); } 398 } 399 //sort candidates by summed point distances 400 sort(candidates->begin(), candidates->end(), CandidateType::SortByDistance); 401 402 res.rankTime = timer.elapsed(); 403 if(param.log) param.log(3,"Ranking performed in %i msec.", res.rankTime); 404 405 //variable needed for progress bar callback 406 float offset = (20.0f/math::Min(param.maxNumShortConsensus,int(candidates->size()))); 407 408 //performs short consensus test on (at most) the first maxNumShortConsensus candidates 409 timer.start(); 410 for(unsigned int j=0; j<candidates->size() && j<param.maxNumShortConsensus; j++) 411 { 412 CandidateType& currCandidate = (*candidates)[j]; 413 Matrix44Type currTr = currCandidate.tr; //load the right transformation 414 Matrix44Type oldTr = ApplyTransformation(mMov, currTr); //apply transformation 415 consParam.samples=param.short_cons_samples; 416 currCandidate.shortCons = cons.Check(consParam); //compute short consensus 417 ResetTransformation(mMov, oldTr); //restore old tranformation 418 419 if(cb){ progBar+=offset; cb(int(progBar),"Short consensus..."); } 420 } 421 res.shortConsTime = timer.elapsed(); 422 if(param.log) param.log(3,"%i short consensus performed in %i msec.",(param.maxNumShortConsensus<int(candidates->size()))? param.maxNumShortConsensus : candidates->size(), res.shortConsTime); 423 424 //sort candidates by short consensus score 425 sort(candidates->begin(), candidates->end(), CandidateType::SortByScore); 426 427 //variables needed for progress bar callback 428 offset = (20.0f/param.maxNumFullConsensus); 429 430 //performs short consensus test on (at most) the first maxNumFullConsensus candidates 431 timer.start(); 432 for(int j=0; j<param.maxNumFullConsensus; j++) 433 { 434 CandidateType& currCandidate = (*candidates)[j]; 435 Matrix44Type currTr = currCandidate.tr; //load the right transformation 436 Matrix44Type oldTr = ApplyTransformation(mMov, currTr); //apply transformation 437 consParam.samples=param.fullConsensusSamples; 438 consParam.threshold = param.consOffset*param.overlap/100.0f; 439 consParam.bestScore = bestConsensus; 440 int consensus = cons.Check(consParam); //compute full consensus 441 ResetTransformation(mMov, oldTr); //restore old tranformation 442 443 //verifies if transformation has the best consensus and update the result 444 if( (consensus >= cons_succ)) 445 { 446 res.numWonFullCons++; 447 if(consensus > bestConsensus) 448 { 449 res.exitCode = Result::ALIGNED; 450 res.tr = currTr; 451 bestConsensus = consensus; 452 res.bestConsensus = 100.0f*(float(bestConsensus)/param.fullConsensusSamples); 453 bestConsIdx = j; 454 if(consensus >= ransac_succ) break; //very good alignment, no more iterations are done 455 } 456 }//else we store the consensus score anyway, so even if we fail we can provide an estimation 457 else{ 458 if(100*(float(consensus)/param.fullConsensusSamples)>res.bestConsensus) 459 res.bestConsensus = 100.0f*(float(consensus)/param.fullConsensusSamples); 460 } 461 if(cb){ progBar+=offset; cb(int(progBar),"Full consensus..."); } 462 } 463 res.fullConsTime = timer.elapsed(); 464 res.totalTime = tot_timer.elapsed(); 465 if(param.log) param.log(3,"%i full consensus performed in %i msec.", param.maxNumFullConsensus, res.fullConsTime); 466 467 //if flag 'points' is checked, clear old picked points and save the new points 468 if(param.pickPoints){ 469 if(bestConsIdx!=-1) StorePickedPoints(mFix, (*candidates)[bestConsIdx].basePtr, mFix.Tr, param.nBase, "B"); 470 else ClearPickedPoints(mFix); 471 if(bestConsIdx!=-1) StorePickedPoints(mMov, (*candidates)[bestConsIdx].matchPtr, (*candidates)[bestConsIdx].tr * mMov.Tr, param.nBase, "M"); 472 else ClearPickedPoints(mMov); 473 } 474 475 //Clean structures... 476 for(int i=0; i<baseVec->size();i++){ delete[] (*baseVec)[i];} 477 for(int i=0; i<candidates->size();i++){ delete[] (*candidates)[i].basePtr; delete[] (*candidates)[i].matchPtr;} 478 delete baseVec; delete candidates; 479 480 return res; 481 } 482 483 /** \brief Return a string describing the error associated to \c code. 484 * 485 * Here is a list of the error codes, with their meaning and caller functions. 486 * \arg error code \c 1 : Error while computing features - Called by Init() 487 * \arg error code \c 2 : Features extracted are not enough to pick a base - Called by Matching() 488 * \arg error code \c 3 : Features extracted are not enough to pick \c k neighbors - Called by Matching() 489 * \arg error code \c 4 : Base isn't enough sparse - Called by Matching() 490 * \arg error code \c 5 : Error while computing rigid transformation - Called by FindRigidTransform() 491 */ getErrorMsg(int code)492 static QString getErrorMsg(int code) 493 { 494 switch(code){ 495 case 1: return QString("Error while computing features."); 496 case 2: return QString("Features extracted are not enough to pick a base."); 497 case 3: return QString("Features extracted are not enough to pick k neighbors."); 498 case 4: return QString("Base isn't enough sparse."); 499 case 5: return QString("Error while computing rigid transformation."); 500 default: return QString("An unkown error occurred."); 501 } 502 } 503 504 /// Set in the struct \c res all the needed stuffs to handle errors setError(int errorCode,Result & res)505 static void setError(int errorCode, Result& res) 506 { 507 res.exitCode = Result::FAILED; 508 res.errorCode = errorCode; 509 res.errorMsg = FeatureAlignment::getErrorMsg(errorCode); 510 } 511 512 /** \brief Performs uniform sampling on a vector of features. 513 * @param vecF The vector of features to be sampled. 514 * @param sampleNum The number of features we would like to sample. It is changed to the number of features actually sampled. 515 * \return An array of pointers to the selected features. 516 */ FeatureUniform(vector<FEATURE_TYPE * > & vecF,int * sampleNum)517 static FEATURE_TYPE** FeatureUniform(vector<FEATURE_TYPE*>& vecF, int* sampleNum) 518 { 519 typedef typename vector<FeatureType*>::iterator FeatureIterator; 520 521 if(*sampleNum>=int(vecF.size())) *sampleNum = vecF.size(); 522 523 vector<FeatureType*> vec; 524 for(FeatureIterator fi=vecF.begin();fi!=vecF.end();++fi) vec.push_back(*fi); 525 526 assert(vec.size()==vecF.size()); 527 528 unsigned int (*p_myrandom)(unsigned int) = tri::SurfaceSampling<MeshType, VertexPointerSampler>::RandomInt; 529 std::random_shuffle(vec.begin(),vec.end(), p_myrandom); 530 531 FeatureType** sampledF = new FeatureType*[*sampleNum]; 532 for(int i =0; i<*sampleNum; ++i) sampledF[i] = vec[i]; 533 534 return sampledF; 535 } 536 537 /** \brief Performs poisson disk sampling on features related to a mesh. Features have to be stored into a per vertex attribute. 538 * @param samplingMesh The mesh to be sampled. 539 * @param sampleNum The number of features we would like to sample. It is changed to the number of features actually sampled. 540 * \return An array of pointers to the selected features. 541 */ FeaturePoisson(MESH_TYPE & samplingMesh,int * sampleNum)542 static FEATURE_TYPE** FeaturePoisson(MESH_TYPE& samplingMesh, int* sampleNum) 543 { 544 typedef typename MeshType::template PerVertexAttributeHandle<FeatureType> PVAttributeHandle; 545 546 VertexPointerSampler samp = VertexPointerSampler(); 547 SampleVertPoissonDisk(samplingMesh, samp, *sampleNum); 548 *sampleNum = samp.sampleVec.size(); //poisson sampling can return untill 50% more of requested samples! 549 PVAttributeHandle fh = FeatureAlignment::GetFeatureAttribute(samplingMesh); 550 if(!tri::Allocator<MeshType>::IsValidHandle(samplingMesh,fh)) return NULL; 551 FeatureType** sampler = new FeatureType*[*sampleNum]; 552 for(int i=0; i<*sampleNum; i++) sampler[i]=&(fh[samp.sampleVec[i]]); 553 return sampler; 554 } 555 556 /** \brief Performs poisson disk sampling on the mesh \c m . It stores pointers to vertexes sampled into \c sampler . 557 * @param m The mesh to be sampled. @param sampler Sampler object where sampled data are stored. 558 * @param sampleNum The target amount of data to sample. 559 */ SampleVertPoissonDisk(MESH_TYPE & m,VertexPointerSampler & sampler,int sampleNum)560 static void SampleVertPoissonDisk(MESH_TYPE& m, VertexPointerSampler& sampler, int sampleNum) 561 { 562 typedef MESH_TYPE MeshType; 563 564 //first of call sampling procedure we have to make sure that mesh has a bbox 565 tri::UpdateBounding<MeshType>::Box(m); 566 567 //setup parameters 568 float radius = tri::SurfaceSampling<MeshType,VertexPointerSampler>::ComputePoissonDiskRadius(m,sampleNum); 569 typename tri::SurfaceSampling<MeshType,VertexPointerSampler>::PoissonDiskParam pp; 570 571 //poisson samplig need a support mesh from which it takes vertexes; we use the same input mesh 572 //as support mesh, so we are sure that pointer to vertexes of input mesh are returned. 573 //perform sampling: number of samples returned can be greater or smaller of the requested amount 574 tri::SurfaceSampling<MeshType,VertexPointerSampler>::Poissondisk(m, sampler, m, radius, pp); 575 } 576 577 /** \brief Retrieve/create an handle to the per vertex attribute associated to a specified feature type. Useful to write short functions. 578 * @param m The mesh in which the attrubute is searched. 579 * @param createAttribute If \c true create the attribute if it doesn't exists. 580 * \return An handle to the attribute, or a NULL handle if it doesn't exists and \createAttribute is \c false. 581 */ 582 static typename MESH_TYPE::template PerVertexAttributeHandle<FEATURE_TYPE> GetFeatureAttribute(MESH_TYPE& m, bool createAttribute = false) 583 { 584 typedef typename MeshType::template PerVertexAttributeHandle<FeatureType> PVAttributeHandle; 585 586 //checks if the attribute exists 587 if(!tri::HasPerVertexAttribute(m,std::string(FeatureType::getName()))){ 588 //if createAttribute is true and attribute doesn't exist, we add it; else return a NULL handle 589 if(createAttribute) tri::Allocator<MeshType>::template AddPerVertexAttribute<FeatureType>(m,std::string(FeatureType::getName())); 590 else return PVAttributeHandle(NULL,0); 591 } 592 //now we can get a handle to the attribute and return it 593 return tri::Allocator<MeshType>::template FindPerVertexAttribute<FeatureType> (m,std::string(FeatureType::getName())); 594 } 595 596 /** \brief Extracts \c numRequested features from mesh \c m using the specified \c samplingStrategy. 597 * What features are actually extracted is a responsability of the Subset() function of the specific feature class. 598 * @param numRequested Number of features to extract. 599 * @param m The mesh in which the attrubute is searched. 600 * @param samplingStrategy Strategy used to sample features. Must be one of \c UNIFORM_SAMPLING, \c POISSON_SAMPLING. 601 * \return A pointer to a vector that contains the extracted featurs, or a NULL pointer if features have not been computed. Vector is allocated inside the function and must be deallocated by the caller. 602 */ 603 static vector<FEATURE_TYPE*>* extractFeatures(int numRequested, MESH_TYPE &m, int samplingStrategy, CallBackPos *cb = NULL) 604 { 605 //Create a vector to hold extracted features 606 vector<FeatureType*>* featureSubset = new vector<FeatureType*>(); 607 //extract numRequested features according to the sampling strategy. 608 //If feature has not been computed previously, return a NULL pointer 609 if(!FeatureType::Subset(numRequested, m, *featureSubset, samplingStrategy, cb)) return NULL; 610 return featureSubset; 611 } 612 613 /** \brief Copy features' descriptions from the vector \c vecF to the array \c dataPts that is used as source for ANN's kdTree. 614 * @param vecF The features' vector. 615 * @param dataPts Array where features' description are copied. It is allocated inside function; the caller have to deallocate mamory. 616 * @param pointDim The dimension of the feature's description. 617 */ SetupKDTreePoints(vector<FEATURE_TYPE * > & vecF,ANNpointArray * dataPts,int pointDim)618 static void SetupKDTreePoints(vector<FEATURE_TYPE*>& vecF, ANNpointArray* dataPts, int pointDim) 619 { 620 //fill dataPts with feature's descriptions in vecF 621 *dataPts = annAllocPts(vecF.size(), pointDim); // allocate data points 622 for(unsigned int i = 0; i < vecF.size(); i++){ 623 for (int j = 0; j < pointDim; j++) 624 (*dataPts)[i][j] = (ANNcoord)(vecF[i]->description[j]); 625 } 626 } 627 628 /** \brief Utility function to deallocate ANN's stuffs. 629 * 630 * Structures are provided as parameters; \c NULL can be used for structures we are not interested to deallocate. 631 * @param kdTree The kdTree structure. 632 * @param dataPts The points used as source by the kdTree. 633 * @param queryPts The points used as queries. 634 * @param idx Indexes to the points in \c dataPts. 635 * @param dists Squared distances of points from the query point. 636 * @param close If \c true the root of the kdTree is used. It should be done only after we are done with ANN. 637 */ CleanKDTree(ANNkd_tree * kdTree,ANNpointArray dataPts,ANNpointArray queryPts,ANNidxArray idx,ANNdistArray dists,bool close)638 static void CleanKDTree(ANNkd_tree* kdTree, ANNpointArray dataPts, ANNpointArray queryPts, ANNidxArray idx, ANNdistArray dists, bool close) 639 { 640 //Cleaning ANN structures 641 if(kdTree){ delete kdTree; kdTree = NULL; } 642 if(dataPts) annDeallocPts(dataPts); 643 if(queryPts) annDeallocPts(queryPts); 644 if(idx){ delete [] idx; idx = NULL; } 645 if(dists){ delete [] dists; dists = NULL; } 646 if(close) annClose(); //done with ANN; clean memory shared among all the kdTrees 647 } 648 649 /** \brief Randomly select a base of \c param.nBase features from vector \c vecFMov and put it in the vector \c baseVec. 650 * 651 * A base is added only if each point is far from the others at least \c param.sparseBaseDist \c * \c param.mMovBBoxDiag/100. 652 * \return 0 if everything is right, an error code according to the getErrorMsg() function otherwise. 653 */ SelectBase(vector<FEATURE_TYPE * > & vecFMov,vector<FEATURE_TYPE ** > & baseVec,Parameters & param)654 static int SelectBase(vector<FEATURE_TYPE*>& vecFMov, vector<FEATURE_TYPE**>& baseVec, Parameters& param) 655 { 656 assert(param.nBase<=int(vecFMov.size())); //not enough features to pick a base 657 float baseDist = param.sparseBaseDist*(param.mMovBBoxDiag/100.0f); //compute needed params 658 659 FeatureType** base = FeatureAlignment::FeatureUniform(vecFMov, &(param.nBase)); //randomly chooses a base of features from vecFFix 660 if(!VerifyBaseDistances(base, param.nBase, baseDist)) return 4; //if base point are not enough sparse, skip 661 baseVec.push_back(base); 662 return 0; 663 } 664 665 /** \brief It performs the matching of each point of the \c base with features contained in the \c kdTree. 666 * 667 * For each base point a kNNSearch is done that finds the 'best' \c param.k features; all these are stored in a vector of \c matches. 668 * \return 0 if everything is right, an error code according to the getErrorMsg() function otherwise. 669 */ 670 static int Matching(vector<FEATURE_TYPE*>& vecFFix, vector<FEATURE_TYPE*>& vecFMov, ANNkd_tree* kdTree, FEATURE_TYPE** base, vector<vector<FeatureType*> >& matches, Parameters& param, CallBackPos *cb=NULL) 671 { 672 float pBar = 0, offset = 100.0f/param.nBase; //used for progresss bar callback 673 if(cb) cb(0, "Matching..."); 674 675 assert(param.k<=int(vecFFix.size())); //not enough features in kdtree to pick k neighbors 676 assert((int)vecFFix.size()>=param.nBase); 677 assert(matches.size()==0); //matches vectors have to be provided empty 678 679 //fill fqueryPts with feature's descriptions in base 680 ANNpointArray fqueryPts; 681 FeatureAlignment::SetupKDTreeQuery(base, param.nBase, &fqueryPts, FeatureType::getFeatureDimension()); 682 683 //additional variables needed 684 ANNidxArray fnnIdx = new ANNidx[param.k]; // allocate near neigh indices 685 ANNdistArray fdists = new ANNdist[param.k]; // allocate near neigh dists 686 vector<FeatureType*> neighbors; 687 //foreach feature in the base find the best matching using fkdTree 688 for(int i = 0; i < param.nBase; i++) 689 { 690 kdTree->annkSearch(fqueryPts[i], param.k, fnnIdx, fdists); //search the k best neighbor for the current point 691 692 assert(fdists[0]!=ANN_DIST_INF); //if this check fails, it means that no feature have been found! 693 694 for(int j=0; j<param.k; j++) neighbors.push_back(vecFFix[fnnIdx[j]]); //store all features 695 matches.push_back(neighbors); //copy neighbors vec into matches vec 696 neighbors.clear(); //make neighbors vec ready for another iteration 697 698 if(cb){ pBar+=offset; cb((int)pBar, "Matching..."); } 699 } 700 701 assert(int(matches.size())==param.nBase); 702 703 //Cleaning ANN structures 704 FeatureAlignment::CleanKDTree(NULL, NULL, fqueryPts, fnnIdx, fdists, false); 705 706 return 0; 707 } 708 709 /** \brief It performs the matching of each point of the \c base with features contained in the \c kdTree. 710 * 711 * For each base point a kNNSearch is done that finds the 'best' \c param.k features; all these are stored in a vector of \c matches. 712 * \return 0 if everything is right, an error code according to the getErrorMsg() function otherwise. 713 */ 714 /* 715 //this is the meatching function that uses clustering 716 static int Matching(vector<FEATURE_TYPE*>& vecFFix, vector<FEATURE_TYPE*>& vecFMov, ANNkd_tree* kdTree, FEATURE_TYPE** base, vector<vector<FeatureType*> >& matches, Parameters& param, CallBackPos *cb=NULL) 717 { 718 float pBar = 0, offset = 100.0f/param.nBase; //used for progresss bar callback 719 if(cb) cb(0, "Matching..."); 720 721 assert(param.k<=int(vecFFix.size())); //not enough features in kdtree to pick k neighbors 722 assert((int)vecFFix.size()>=param.nBase); 723 assert(matches.size()==0); //matches vectors have to be provided empty 724 725 //fill fqueryPts with feature's descriptions in base 726 ANNpointArray fqueryPts; 727 FeatureAlignment::SetupKDTreeQuery(base, param.nBase, &fqueryPts, FeatureType::getFeatureDimension()); 728 729 MyMesh clusterMesh; 730 tri::Allocator<MyMesh>::AddVertices(clusterMesh,param.k); 731 SimpleTempData<MyMesh::VertContainer, FeatureType*> MyTempData(clusterMesh.vert); 732 733 //additional variables needed 734 ANNidxArray fnnIdx = new ANNidx[param.k]; // allocate near neigh indices 735 ANNdistArray fdists = new ANNdist[param.k]; // allocate near neigh dists 736 vector<FeatureType*> neighbors; 737 tri::Clustering<MyMesh, tri::NearestToCenter<MyMesh> > ClusteringGrid; 738 float cellsize = param.consensusDist*(param.mMovBBoxDiag/100.0f); //compute needed params 739 //foreach feature in the base find the best matching using fkdTree 740 for(int i = 0; i < param.nBase; i++) 741 { 742 kdTree->annkSearch(fqueryPts[i], param.k, fnnIdx, fdists); //search the k best neighbor for the current point 743 744 assert(fdists[0]!=ANN_DIST_INF); //if this check fails, it means that no feature have been found! 745 746 int j; MyMesh::VertexIterator vi; 747 for(vi=clusterMesh.vert.begin(), j=0; vi<clusterMesh.vert.end(), j<param.k; vi++, j++){ 748 vi->P() = vecFFix[fnnIdx[j]]->pos; 749 vi->N() = vecFFix[fnnIdx[j]]->normal; 750 MyTempData[vi] = vecFFix[fnnIdx[j]]; 751 } 752 if(param.log) param.log(3,"clusterMesh contains %i vertexes",j); 753 ClusteringGrid.Init(clusterMesh.bbox,0,cellsize); 754 ClusteringGrid.AddPointSet(clusterMesh); 755 ClusteringGrid.SelectPointSet(clusterMesh); 756 for(vi=clusterMesh.vert.begin(); vi<clusterMesh.vert.end(); vi++){ 757 if(vi->IsS()) neighbors.push_back(MyTempData[vi]); 758 } 759 if(param.log) param.log(3,"neighbors.size %i",neighbors.size()); 760 matches.push_back(neighbors); //copy neighbors vec into matches vec 761 neighbors.clear(); //make neighbors vec ready for another iteration 762 763 if(cb){ pBar+=offset; cb((int)pBar, "Matching..."); } 764 } 765 766 assert(int(matches.size())==param.nBase); 767 768 //Cleaning ANN structures 769 FeatureAlignment::CleanKDTree(NULL, NULL, fqueryPts, fnnIdx, fdists, false); 770 771 return 0; 772 } 773 */ 774 /** \brief Given \c param.nbase sets of matching points, explore the space of all the matching bases using a Branch And Bound algorithm and adds 775 * the admisible bases into a vector of candidates. 776 * 777 * \return 0 if everything is right, an error code according to the getErrorMsg() function otherwise. 778 */ BranchAndBound(FEATURE_TYPE ** base,vector<vector<FeatureType * >> & matches,vector<CandidateType> & candidates,Parameters & param)779 static int BranchAndBound(FEATURE_TYPE** base, vector<vector<FeatureType*> >& matches, vector<CandidateType>& candidates, Parameters& param) 780 { 781 //compute needed params 782 float errDist = param.mutualErrDist*(param.mMovBBoxDiag/100.0f); 783 784 //branch and bound 785 int curSolution[param.nBase]; 786 for(int i=0; i<param.nBase; i++) curSolution[i] = 0; //initialization 787 FeatureAlignment::Match(base, matches, param.nBase, 0, curSolution, candidates, errDist); 788 return 0; 789 } 790 791 /** \brief Compute the rigid transformation matrix that aligns the base \c movF with the matching base \c fixF and put the matrix into \c tr. 792 * 793 * \return 0 if everything is right, an error code according to the getErrorMsg() function otherwise. 794 */ 795 static int FindRigidTransformation(MESH_TYPE& mFix, MESH_TYPE& mMov, FEATURE_TYPE* fixF[], FEATURE_TYPE* movF[], int nBase, Matrix44<typename MESH_TYPE::ScalarType>& tr, CallBackPos *cb=NULL) 796 { 797 if(cb) cb(0,"Computing rigid transformation..."); 798 799 //computes the rigid transformation matrix that overlaps the two points sets 800 vector< Point3<ScalarType> > fixPoints; 801 vector< Point3<ScalarType> > movPoints; 802 803 for(int i = 0; i<nBase; i++) 804 { 805 movPoints.push_back(mMov.Tr * fixF[i]->pos); //points on mFix 806 fixPoints.push_back(mFix.Tr * movF[i]->pos); //points on mMov 807 } 808 809 //this compute transformation that brings movPoints over fixPoints 810 bool ok = PointMatching<ScalarType>::ComputeRigidMatchMatrix(tr, fixPoints, movPoints); 811 if(!ok) return 5; //Error while computing rigid transformation 812 813 return 0; 814 } 815 816 static void AddPickedPoints(MESH_TYPE& m, vector<FEATURE_TYPE*>& vecF, char* prefix = NULL) 817 { 818 typedef MESH_TYPE MeshType; 819 typedef FEATURE_TYPE FeatureType; 820 typedef typename MeshType::template PerMeshAttributeHandle<PickedPoints*> PMAttributeHandle; 821 822 PMAttributeHandle pph; 823 //if attribute doesn't exist, we add it; then we can get a handle to the attribute 824 if(!tri::HasPerMeshAttribute(m, PickedPoints::Key)){ 825 pph = tri::Allocator<MeshType>::template AddPerMeshAttribute<PickedPoints*>(m,PickedPoints::Key); 826 pph() = new PickedPoints(); 827 } 828 else pph = tri::Allocator<MeshType>::template FindPerMeshAttribute<PickedPoints*>(m, PickedPoints::Key); 829 830 for(unsigned int i=0; i<vecF.size(); i++){ 831 //build up a point name made of an id and feature value... 832 char name[100]; int j, limit; 833 if(prefix==NULL) prefix=""; 834 limit = sprintf(name,"%s%i - ",prefix,i); 835 for(j=0; j<FeatureType::getFeatureDimension()-1; j++){ 836 limit += sprintf(name+limit,"%.2f,",vecF[i]->description[j]); 837 } 838 sprintf(name+limit,"%.2f",vecF[i]->description[j]); 839 840 pph()->addPoint(name, m.Tr * vecF[i]->pos, true); //add point 841 } 842 } 843 ClearPickedPoints(MESH_TYPE & m)844 static void ClearPickedPoints(MESH_TYPE& m) 845 { 846 typedef MESH_TYPE MeshType; 847 typedef typename MeshType::template PerMeshAttributeHandle<PickedPoints*> PMAttributeHandle; 848 849 PMAttributeHandle pph; 850 //we get a handle to the attribute if it exists 851 if(!tri::HasPerMeshAttribute(m, PickedPoints::Key)) return; 852 pph = tri::Allocator<MeshType>::template FindPerMeshAttribute<PickedPoints*>(m, PickedPoints::Key); 853 854 //delete previous picked points 855 (pph()->getPickedPointVector())->clear(); 856 } 857 858 private: 859 860 static void StorePickedPoints(MESH_TYPE& m, FEATURE_TYPE* vecF[], Matrix44<typename MESH_TYPE::ScalarType> tr, int size, char* prefix = NULL) 861 { 862 typedef MESH_TYPE MeshType; 863 typedef FEATURE_TYPE FeatureType; 864 typedef typename MeshType::template PerMeshAttributeHandle<PickedPoints*> PMAttributeHandle; 865 866 PMAttributeHandle pph; 867 //if attribute doesn't exist, we add it; then we can get a handle to the attribute 868 if(!tri::HasPerMeshAttribute(m, PickedPoints::Key)){ 869 pph = tri::Allocator<MeshType>::template AddPerMeshAttribute<PickedPoints*>(m,PickedPoints::Key); 870 pph() = new PickedPoints(); 871 } 872 else pph = tri::Allocator<MeshType>::template FindPerMeshAttribute<PickedPoints*>(m, PickedPoints::Key); 873 874 (pph()->getPickedPointVector())->clear(); //clear old contents 875 876 for(int i=0; i<size; i++){ 877 //build up a point name made of an id and feature value... 878 char name[100]; int j, limit; 879 if(prefix==NULL) prefix = ""; 880 limit = sprintf(name,"%s%i - ",prefix,i); 881 for(j=0; j<FeatureType::getFeatureDimension()-1; j++){ 882 limit += sprintf(name+limit,"%.2f,",vecF[i]->description[j]); 883 } 884 sprintf(name+limit,"%.2f",vecF[i]->description[j]); 885 886 pph()->addPoint(name, tr * vecF[i]->pos, true); //add point 887 } 888 } 889 SetupKDTreeQuery(FEATURE_TYPE * queryPointsArray[],int queryPointsSize,ANNpointArray * queryPts,int pointDim)890 static void SetupKDTreeQuery(FEATURE_TYPE* queryPointsArray[], int queryPointsSize, ANNpointArray* queryPts, int pointDim) 891 { 892 //fill query points with feature's description 893 *queryPts = annAllocPts(queryPointsSize, pointDim); //allocate query points 894 for (int i = 0; i < queryPointsSize; i++){ 895 for (int j = 0; j < pointDim; j++) 896 (*queryPts)[i][j] = (ANNcoord)(queryPointsArray[i]->description[j]); 897 } 898 } 899 900 static void Match(FEATURE_TYPE** base, vector<vector<FEATURE_TYPE*> >& matches, int nBase, int level, int curSolution[], vector<CandidateType>& candidates, float errDist, CallBackPos *cb = NULL) 901 { 902 assert(level<nBase); 903 904 for(unsigned int j=0; j<matches[level].size(); j++){ 905 curSolution[level] = j; 906 if(MatchSolution(base, matches, level, curSolution, errDist)){ 907 if(level==nBase-1){ 908 FeatureType** solution = new FeatureType*[nBase]; 909 for(int h=0; h<nBase; h++){ 910 solution[h] = matches[h][curSolution[h]]; 911 } 912 candidates.push_back(CandidateType(base,solution)); 913 } 914 else 915 Match(base, matches, nBase, level+1, curSolution, candidates, errDist); 916 } 917 } 918 curSolution[level] = 0; 919 } 920 MatchSolution(FEATURE_TYPE ** base,vector<vector<FEATURE_TYPE * >> & matches,int level,int curSolution[],float errDist)921 static bool MatchSolution(FEATURE_TYPE** base, vector<vector<FEATURE_TYPE*> >& matches, int level, int curSolution[], float errDist) 922 { 923 if (level==0) return true; 924 925 for(int j=0; j<level; j++){ 926 float distF = Distance(base[level]->pos, base[j]->pos); 927 float distM = Distance(matches[level][curSolution[level]]->pos, matches[j][curSolution[j]]->pos); 928 if( math::Abs(distF-distM)>errDist) return false; 929 } 930 return true; 931 } 932 933 //Verify that all points in a base are enough sparse VerifyBaseDistances(FEATURE_TYPE * base[],int nBase,float baseDist)934 static bool VerifyBaseDistances(FEATURE_TYPE* base[], int nBase, float baseDist ) 935 { 936 typedef FEATURE_TYPE FeatureType; 937 938 for(int i=0; i<nBase-1; i++){ 939 for(int j=i+1; j<nBase; j++){ 940 if(Distance(base[i]->pos, base[j]->pos)<=baseDist) return false; 941 } 942 } 943 return true; 944 } 945 ApplyTransformation(MESH_TYPE & m,Matrix44<typename MESH_TYPE::ScalarType> & tr)946 static Matrix44<typename MESH_TYPE::ScalarType> ApplyTransformation(MESH_TYPE& m, Matrix44<typename MESH_TYPE::ScalarType>& tr) 947 { 948 typedef MESH_TYPE MeshType; 949 typedef typename MeshType::ScalarType ScalarType; 950 951 Matrix44<ScalarType> oldMat = m.Tr; //save old transformation matrix of m 952 m.Tr = tr * m.Tr; //apply transformation 953 return oldMat; //return the old transformation matrix 954 } 955 ResetTransformation(MESH_TYPE & m,Matrix44<typename MESH_TYPE::ScalarType> & tr)956 static void ResetTransformation(MESH_TYPE& m, Matrix44<typename MESH_TYPE::ScalarType>& tr) 957 { 958 m.Tr = tr; 959 } 960 SummedPointDistances(MESH_TYPE & mFix,MESH_TYPE & mMov,FEATURE_TYPE * baseF[],FEATURE_TYPE * matchF[],int nBase)961 static float SummedPointDistances(MESH_TYPE& mFix, MESH_TYPE& mMov, FEATURE_TYPE* baseF[], FEATURE_TYPE* matchF[], int nBase) 962 { 963 typedef MESH_TYPE MeshType; 964 typedef typename MeshType::ScalarType ScalarType; 965 966 Matrix33<ScalarType> matMov(mMov.Tr,3); //3x3 matrix needed totransform normals 967 Matrix33<ScalarType> matFix(mFix.Tr,3); //3x3 matrix needed totransform normals 968 Point3<ScalarType> fixNrm, movNrm; //normals 969 float spd, snd, result = 0.0f; 970 971 for(int i=0; i<nBase;i++){ 972 //transform normals properly; normals inside features are yet normalized 973 fixNrm = matFix * matchF[i]->normal; 974 movNrm = matMov * baseF[i]->normal; 975 976 //compute distance beetween points and then beetween normals 977 spd = Distance( mFix.Tr * matchF[i]->pos, mMov.Tr * baseF[i]->pos ); 978 snd = math::Clamp(1-fixNrm.dot(movNrm),0.0f,1.0f); 979 980 //weight distances with normals and update result 981 result+=spd*snd; 982 } 983 return result; 984 } 985 }; 986 #endif //FEATURE_ALIGNMENT_H 987