1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004 \/)\/ *
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
24 #include "AlignPair.h"
25 #include "../point_matching_scale.h"
26
27 #include <vcg/complex/algorithms/clean.h>
28 #include <vcg/complex/algorithms/update/position.h>
29 #include <vcg/complex/algorithms/update/component_ep.h>
30 #include <vcg/complex/algorithms/update/flag.h>
31 #include <vcg/complex/algorithms/update/normal.h>
32 #include <vcg/complex/algorithms/update/bounding.h>
33 #include <vcg/complex/algorithms/closest.h>
34 #include <wrap/io_trimesh/import.h>
35 #include <wrap/io_trimesh/export_ply.h>
36
37 #include <vcg/math/random_generator.h>
38 #include <vcg/math/gen_normal.h>
39 #include <vcg/space/point_matching.h>
40
41 using namespace vcg;
42 using namespace std;
43
Import(const char * filename,Matrix44d & Tr)44 bool AlignPair::A2Mesh::Import(const char *filename, Matrix44d &Tr)
45 {
46 int err = tri::io::Importer<A2Mesh>::Open(*this, filename);
47 if (err) {
48 printf("Error in reading %s: '%s'\n", filename, tri::io::Importer<A2Mesh>::ErrorMsg(err));
49 exit(-1);
50 }
51 printf("read mesh `%s'\n", filename);
52 return Init(Tr);
53 }
54
InitVert(const Matrix44d & Tr)55 bool AlignPair::A2Mesh::InitVert(const Matrix44d &Tr)
56 {
57 Matrix44d Idn; Idn.SetIdentity();
58 if (Tr != Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this, Tr);
59 tri::UpdateNormal<A2Mesh>::NormalizePerVertex(*this);
60 tri::UpdateBounding<A2Mesh>::Box(*this);
61 return true;
62 }
63
Init(const Matrix44d & Tr)64 bool AlignPair::A2Mesh::Init(const Matrix44d &Tr)
65 {
66 Matrix44d Idn; Idn.SetIdentity();
67 tri::Clean<A2Mesh>::RemoveUnreferencedVertex(*this);
68 if (Tr != Idn) tri::UpdatePosition<A2Mesh>::Matrix(*this, Tr);
69 tri::UpdateNormal<A2Mesh>::PerVertexNormalizedPerFaceNormalized(*this);
70 tri::UpdateFlags<A2Mesh>::FaceBorderFromNone(*this);
71 tri::UpdateBounding<A2Mesh>::Box(*this);
72
73 return true;
74 }
75
76
clear()77 void AlignPair::Stat::clear()
78 {
79 I.clear();
80 StartTime = 0;
81 MovVertNum = 0;
82 FixVertNum = 0;
83 FixFaceNum = 0;
84 }
85
86 // Restituisce true se nelle ultime <lastiter> iterazioni non e' diminuito
87 // l'errore
Stable(int lastiter)88 bool AlignPair::Stat::Stable(int lastiter)
89 {
90 if (I.empty()) return false;
91 int parag = int(I.size()) - lastiter;
92
93 if (parag < 0) parag = 0;
94 if (I.back().pcl50 < I[parag].pcl50) return false; // se siamo diminuiti non e' stabile
95
96 return true;
97
98 }
99
100
Dump(FILE * fp)101 void AlignPair::Stat::Dump(FILE *fp)
102 {
103 if (I.size() == 0) {
104 fprintf(fp, "Empty AlignPair::Stat\n");
105 return;
106 }
107 fprintf(fp, "Final Err %8.5f In %i iterations Total Time %ims\n", LastPcl50(), (int)I.size(), TotTime());
108 fprintf(fp, "Mindist Med Hi Avg RMS StdDev Time Tested Used Dist Bord Angl \n");
109 for (unsigned int qi = 0; qi < I.size(); ++qi)
110 fprintf(fp, "%5.2f (%6.3f:%6.3f) (%6.3f %6.3f %6.3f) %4ims %5i %5i %4i+%4i+%4i\n",
111 I[qi].MinDistAbs,
112 I[qi].pcl50, I[qi].pclhi,
113 I[qi].AVG, I[qi].RMS, I[qi].StdDev,
114 IterTime(qi),
115 I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded);
116 }
117
118 // Scrive una tabella con tutti i valori
HTMLDump(FILE * fp)119 void AlignPair::Stat::HTMLDump(FILE *fp)
120 {
121 fprintf(fp, "Final Err %8.5f In %i iterations Total Time %ims\n", LastPcl50(), (int)I.size(), TotTime());
122 fprintf(fp, "<table border>\n");
123 fprintf(fp, "<tr> <th>Mindist</th><th> 50ile </th><th> Hi </th><th> Avg </th><th> RMS </th><th> StdDev </th><th> Time </th><th> Tested </th><th> Used </th><th> Dist </th><th> Bord </th><th> Angl \n");
124 for (unsigned int qi = 0; qi < I.size(); ++qi)
125 fprintf(fp, "<tr> <td> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %5.3f </td><td align=\"right\"> %8.5f </td><td align=\"right\"> %9.6f </td><td align=\"right\"> %4ims </td><td align=\"right\"> %5i </td><td align=\"right\"> %5i </td><td align=\"right\"> %4i </td><td align=\"right\"> %4i </td><td align=\"right\">%4i </td><td align=\"right\"></tr>\n",
126 I[qi].MinDistAbs,
127 I[qi].pcl50, I[qi].pclhi,
128 I[qi].AVG, I[qi].RMS, I[qi].StdDev,
129 IterTime(qi),
130 I[qi].SampleTested, I[qi].SampleUsed, I[qi].DistanceDiscarded, I[qi].BorderDiscarded, I[qi].AngleDiscarded);
131 fprintf(fp, "</table>\n");
132 }
133
134
135
136 /*
137 This function is used to choose remove outliers after each ICP iteration.
138 All the points with a distance over the given Percentile are discarded.
139 It uses two parameters
140 MaxPointNum an (unused) hard limit on the number of points that are chosen
141 MinPointNum the minimum number of points that have to be chosen to be usable
142
143 */
ChoosePoints(vector<Point3d> & Ps,vector<Point3d> & Ns,vector<Point3d> & Pt,vector<Point3d> & OPt,double PassHi,Histogramf & H)144 bool AlignPair::ChoosePoints(vector<Point3d> &Ps, // vertici corrispondenti su src (rossi)
145 vector<Point3d> &Ns, // normali corrispondenti su src (rossi)
146 vector<Point3d> &Pt, // vertici scelti su trg (verdi)
147 vector<Point3d> &OPt, // vertici scelti su trg (verdi)
148 double PassHi,
149 Histogramf &H)
150 {
151 const int N = ap.MaxPointNum;
152 double newmaxd = H.Percentile(float(PassHi));
153 //printf("%5.1f of the pairs has a distance less than %g and greater than %g (0..%g) avg %g\n", Perc*100,newmind,newmaxd,H.maxv,H.Percentile(.5));
154 int sz = int(Ps.size());
155 int fnd = 0;
156 int lastgood = sz - 1;
157 math::SubtractiveRingRNG myrnd;
158 while (fnd < N && fnd < lastgood)
159 {
160 int index = fnd + myrnd.generate(lastgood - fnd);
161 double dd = Distance(Ps[index], Pt[index]);
162 if (dd <= newmaxd)
163 {
164 swap(Ps[index], Ps[fnd]);
165 swap(Ns[index], Ns[fnd]);
166 swap(Pt[index], Pt[fnd]);
167 swap(OPt[index], OPt[fnd]);
168 ++fnd;
169 }
170 else
171 {
172 swap(Ps[index], Ps[lastgood]);
173 swap(Ns[index], Ns[lastgood]);
174 swap(Pt[index], Pt[lastgood]);
175 swap(OPt[index], OPt[lastgood]);
176 lastgood--;
177 }
178 }
179 Ps.resize(fnd);
180 Ns.resize(fnd);
181 Pt.resize(fnd);
182 OPt.resize(fnd);
183 // printf("Scelte %i coppie tra le %i iniziali, scartate quelle con dist > %f\n", fnd, sz, newmaxd);
184
185 if ((int)Ps.size() < ap.MinPointNum) {
186 printf("Troppi pochi punti!\n");
187 Ps.clear();
188 Ns.clear();
189 Pt.clear();
190 OPt.clear();
191 return false;
192 }
193 return true;
194 }
195
196 /*
197 Funzione chiamata dalla Align ad ogni ciclo
198 Riempie i vettori <MovVert> e <MovNorm> con i coordinate e normali presi dal vettore di vertici mov
199 della mesh da muovere trasformata secondo la matrice <In>
200 Calcola anche il nuovo bounding box di tali vertici trasformati.
201 */
InitMov(vector<Point3d> & MovVert,vector<Point3d> & MovNorm,Box3d & trgbox,const Matrix44d & in)202 bool AlignPair::InitMov(
203 vector< Point3d > &MovVert,
204 vector< Point3d > &MovNorm,
205 Box3d &trgbox,
206 const Matrix44d &in) // trasformazione Iniziale (che porta i punti di trg su src)
207 {
208 Point3d pp, nn;
209
210 MovVert.clear();
211 MovNorm.clear();
212 trgbox.SetNull();
213
214 A2Mesh::VertexIterator vi;
215 for (vi = mov->begin(); vi != mov->end(); vi++) {
216 pp = in*(*vi).P();
217 nn = in*Point3d((*vi).P() + (*vi).N()) - pp;
218 nn.Normalize();
219 MovVert.push_back(pp);
220 MovNorm.push_back(nn);
221 trgbox.Add(pp);
222 }
223 return true;
224 }
225
InitFixVert(AlignPair::A2Mesh * fm,AlignPair::Param & pp,A2GridVert & u,int PreferredGridSize)226 bool AlignPair::InitFixVert(AlignPair::A2Mesh *fm,
227 AlignPair::Param &pp,
228 A2GridVert &u,
229 int PreferredGridSize)
230 {
231 Box3d bb2 = fm->bbox;
232 double MinDist = pp.MinDistAbs*1.1;
233 //the bbox of the grid should be enflated of the mindist used in the ICP search
234 bb2.Offset(Point3d(MinDist, MinDist, MinDist));
235
236 u.SetBBox(bb2);
237
238 //Inserisco la src nella griglia
239 if (PreferredGridSize == 0) PreferredGridSize = int(fm->vert.size())*pp.UGExpansionFactor;
240 u.Set(fm->vert.begin(), fm->vert.end());//, PreferredGridSize);
241 printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]);
242 return true;
243 }
244
245
InitFix(AlignPair::A2Mesh * fm,AlignPair::Param & pp,A2Grid & u,int PreferredGridSize)246 bool AlignPair::InitFix(AlignPair::A2Mesh *fm,
247 AlignPair::Param &pp,
248 A2Grid &u,
249 int PreferredGridSize)
250 {
251 tri::InitFaceIMark(*fm);
252
253 Box3d bb2 = fm->bbox;
254 // double MinDist= fm->bbox.Diag()*pp.MinDistPerc;
255 double MinDist = pp.MinDistAbs*1.1;
256 //gonfio della distanza utente il BBox della seconda mesh
257 bb2.Offset(Point3d(MinDist, MinDist, MinDist));
258
259 u.SetBBox(bb2);
260
261 //Inserisco la src nella griglia
262 if (PreferredGridSize == 0) PreferredGridSize = int(fm->face.size())*pp.UGExpansionFactor;
263 u.Set(fm->face.begin(), fm->face.end(), PreferredGridSize);
264 printf("UG %i %i %i\n", u.siz[0], u.siz[1], u.siz[2]);
265 return true;
266 }
267 /*
268 The Main ICP alignment Function:
269 It assumes that:
270 we have two meshes:
271 - Fix the mesh that does not move and stays in the spatial indexing structure.
272 - Mov the mesh we 'move' e.g. the one for which we search the transforamtion.
273
274 requires normalize normals for vertices AND faces
275 Allinea due mesh;
276 Assume che:
277 la uniform grid sia gia' inizializzata con la mesh fix
278
279
280 */
281
Align(A2Grid & u,A2GridVert & uv,const Matrix44d & in,Matrix44d & out,vector<Point3d> & Pfix,vector<Point3d> & Nfix,vector<Point3d> & OPmov,vector<Point3d> & ONmov,Histogramf & H,AlignPair::Stat & as)282 bool AlignPair::Align(
283 A2Grid &u,
284 A2GridVert &uv,
285 const Matrix44d &in, // trasformazione Iniziale (che porta i punti di mov su fix)
286 Matrix44d &out, // trasformazione calcolata
287 vector<Point3d> &Pfix, // vertici corrispondenti su src (rossi)
288 vector<Point3d> &Nfix, // normali corrispondenti su src (rossi)
289 vector<Point3d> &OPmov, // vertici scelti su trg (verdi) prima della trasformazione in ingresso (Original Point Target)
290 vector<Point3d> &ONmov, // normali scelti su trg (verdi)
291 Histogramf &H,
292 AlignPair::Stat &as)
293 {
294 vector<char> beyondCntVec; // vettore per marcare i movvert che sicuramente non si devono usare
295 // ogni volta che un vertice si trova a distanza oltre max dist viene incrementato il suo contatore;
296 // i movvert che sono stati scartati piu' di MaxCntDist volte non si guardano piu';
297 const int maxBeyondCnt = 3;
298 vector< Point3d > movvert;
299 vector< Point3d > movnorm;
300 vector<Point3d> Pmov; // vertici scelti dopo la trasf iniziale
301 status = SUCCESS;
302 int tt0 = clock();
303
304 out = in;
305
306 int i;
307
308 double CosAngleThr = cos(ap.MaxAngleRad);
309 double StartMinDist = ap.MinDistAbs;
310 int tt1 = clock();
311 int ttsearch = 0;
312 int ttleast = 0;
313 int nc = 0;
314 as.clear();
315 as.StartTime = clock();
316
317 beyondCntVec.resize(mov->size(), 0);
318
319 /**************** BEGIN ICP LOOP ****************/
320 do
321 {
322 Stat::IterInfo ii;
323 Box3d movbox;
324 InitMov(movvert, movnorm, movbox, out);
325 H.SetRange(0.0f, float(StartMinDist), 512, 2.5f);
326 Pfix.clear();
327 Nfix.clear();
328 Pmov.clear();
329 OPmov.clear();
330 ONmov.clear();
331 int tts0 = clock();
332 ii.MinDistAbs = StartMinDist;
333 int LocSampleNum = min(ap.SampleNum, int(movvert.size()));
334 Box3d fixbox;
335 if (u.Empty()) fixbox = uv.bbox;
336 else fixbox = u.bbox;
337 for (i = 0; i < LocSampleNum; ++i)
338 {
339 if (beyondCntVec[i] < maxBeyondCnt)
340 {
341
342 if (fixbox.IsIn(movvert[i]))
343 {
344 double error = StartMinDist;
345 Point3d closestPoint, closestNormal;
346 double maxd = StartMinDist;
347 ii.SampleTested++;
348 if (u.Empty()) // using the point cloud grid
349 {
350 A2Mesh::VertexPointer vp = tri::GetClosestVertex(*fix, uv, movvert[i], maxd, error);
351 if (error >= StartMinDist) {
352 ii.DistanceDiscarded++; ++beyondCntVec[i]; continue;
353 }
354 if (movnorm[i].dot(vp->N()) < CosAngleThr) {
355 ii.AngleDiscarded++; continue;
356 }
357 closestPoint = vp->P();
358 closestNormal = vp->N();
359 }
360 else // using the standard faces and grid
361 {
362 A2Mesh::FacePointer f = vcg::tri::GetClosestFaceBase<vcg::AlignPair::A2Mesh, vcg::AlignPair::A2Grid >(*fix, u, movvert[i], maxd, error, closestPoint);
363 if (error >= StartMinDist) {
364 ii.DistanceDiscarded++; ++beyondCntVec[i]; continue;
365 }
366 if (movnorm[i].dot(f->N()) < CosAngleThr) {
367 ii.AngleDiscarded++; continue;
368 }
369 Point3d ip;
370 InterpolationParameters<A2Face, double>(*f, f->N(), closestPoint, ip);
371 const double IP_EPS = 0.00001;
372 // If ip[i] == 0 it means that we are on the edge opposite to i
373 if ((fabs(ip[0]) <= IP_EPS && f->IsB(1)) || (fabs(ip[1]) <= IP_EPS && f->IsB(2)) || (fabs(ip[2]) <= IP_EPS && f->IsB(0))){
374 ii.BorderDiscarded++; continue;
375 }
376 closestNormal = f->N();
377 }
378 // The sample was accepted. Store it.
379 Pmov.push_back(movvert[i]);
380 OPmov.push_back((*mov)[i].P());
381 ONmov.push_back((*mov)[i].N());
382 Nfix.push_back(closestNormal);
383 Pfix.push_back(closestPoint);
384 H.Add(float(error));
385 ii.SampleUsed++;
386 }
387 else
388 beyondCntVec[i] = maxBeyondCnt + 1;
389 }
390 } // End for each pmov
391 int tts1 = clock();
392 //printf("Found %d pairs\n",(int)Pfix.size());
393 if (!ChoosePoints(Pfix, Nfix, Pmov, OPmov, ap.PassHiFilter, H))
394 {
395 if (int(Pfix.size()) < ap.MinPointNum)
396 {
397 status = TOO_FEW_POINTS;
398 ii.Time = clock();
399 as.I.push_back(ii);
400 return false;
401 }
402 }
403 Matrix44d newout;
404 switch (ap.MatchMode)
405 {
406 case AlignPair::Param::MMSimilarity: ComputeRotoTranslationScalingMatchMatrix(newout, Pfix, Pmov); break;
407 case AlignPair::Param::MMRigid: ComputeRigidMatchMatrix(Pfix, Pmov, newout); break;
408 default:
409 status = UNKNOWN_MODE;
410 ii.Time = clock();
411 as.I.push_back(ii);
412 return false;
413 }
414
415 // double sum_before=0;
416 // double sum_after=0;
417 // for(unsigned int iii=0;iii<Pfix.size();++iii)
418 // {
419 // sum_before+=Distance(Pfix[iii], out*OPmov[iii]);
420 // sum_after+=Distance(Pfix[iii], newout*OPmov[iii]);
421 // }
422 // //printf("Distance %f -> %f\n",sum_before/double(Pfix.size()),sum_after/double(Pfix.size()) ) ;
423
424 // le passate successive utilizzano quindi come trasformazione iniziale questa appena trovata.
425 // Nei prossimi cicli si parte da questa matrice come iniziale.
426 out = newout * out;
427
428 assert(Pfix.size() == Pmov.size());
429 int tts2 = clock();
430 ttsearch += tts1 - tts0;
431 ttleast += tts2 - tts1;
432 ii.pcl50 = H.Percentile(.5);
433 ii.pclhi = H.Percentile(float(ap.PassHiFilter));
434 ii.AVG = H.Avg();
435 ii.RMS = H.RMS();
436 ii.StdDev = H.StandardDeviation();
437 ii.Time = clock();
438 as.I.push_back(ii);
439 nc++;
440 // The distance of the next points to be considered is lowered according to the <ReduceFactor> parameter.
441 // We use 5 times the <ReduceFactor> percentile of the found points.
442 if (ap.ReduceFactorPerc<1) StartMinDist = max(ap.MinDistAbs*ap.MinMinDistPerc, min(StartMinDist, 5.0*H.Percentile(float(ap.ReduceFactorPerc))));
443 } while (
444 nc <= ap.MaxIterNum &&
445 H.Percentile(.5) > ap.TrgDistAbs &&
446 (nc<ap.EndStepNum + 1 || !as.Stable(ap.EndStepNum))
447 );
448 /**************** END ICP LOOP ****************/
449 int tt2 = clock();
450 out[3][0] = 0; out[3][1] = 0; out[3][2] = 0; out[3][3] = 1;
451 Matrix44d ResCopy = out;
452 Point3d scv, shv, rtv, trv;
453 Decompose(ResCopy, scv, shv, rtv, trv);
454 if ((ap.MatchMode == vcg::AlignPair::Param::MMRigid) && (math::Abs(1 - scv[0])>ap.MaxScale || math::Abs(1 - scv[1]) > ap.MaxScale || math::Abs(1 - scv[2]) > ap.MaxScale)) {
455 status = TOO_MUCH_SCALE;
456 return false;
457 }
458 if (shv[0] > ap.MaxShear || shv[1] > ap.MaxShear || shv[2] > ap.MaxShear) {
459 status = TOO_MUCH_SHEAR;
460 return false;
461 }
462 printf("Grid %i %i %i - fn %i\n", u.siz[0], u.siz[1], u.siz[2], fix->fn);
463 printf("Init %8.3f Loop %8.3f Search %8.3f least sqrt %8.3f\n",
464 float(tt1 - tt0) / CLOCKS_PER_SEC, float(tt2 - tt1) / CLOCKS_PER_SEC,
465 float(ttsearch) / CLOCKS_PER_SEC, float(ttleast) / CLOCKS_PER_SEC);
466
467 return true;
468 }
469
470
471
472
ErrorMsg(ErrorCode code)473 const char *AlignPair::ErrorMsg(ErrorCode code)
474 {
475 switch (code){
476 case SUCCESS: return "Success ";
477 case NO_COMMON_BBOX: return "No Common BBox ";
478 case TOO_FEW_POINTS: return "Too few points ";
479 case LSQ_DIVERGE: return "LSQ not converge";
480 case TOO_MUCH_SHEAR: return "Too much shear ";
481 case TOO_MUCH_SCALE: return "Too much scale ";
482 case UNKNOWN_MODE: return "Unknown mode ";
483 default: assert(0); return "Catastrophic Error";
484 }
485 return 0;
486 }
487 /*
488
489 Questa parte era relativa all'allineatore automatico.
490 Da controllare se ancora funge.
491
492
493 // Calcola la migliore traslazione possibile,
494 // cioe' quella dove i punti in movvert cascano per la maggior parte
495 // in celle della ug occupate dalla fixmesh
496 // Restituisce il numero di celle di sovrapposizione massimo trovato
497
498 int AlignPair::SearchTranslate(A2Grid &u,
499 const Matrix44d &BaseRot,
500 const int range,
501 const int step,
502 Point3d &BestTransV, // miglior vettore di spostamento
503 bool Verbose)
504 {
505 const int wide1=(range*2+1);
506 const int wide2=wide1*wide1;
507 vector< Point3d > movvert;
508 vector< Point3d > movnorm;
509 Box3d movbox;
510
511 int t0=clock();
512 InitMov(movvert,movnorm,movbox,BaseRot);
513 Point3i ip;
514 int i,ii,jj,kk;
515 vector<int> test((range*2+1)*(range*2+1)*(range*2+1),0);
516 //int bx,by,bz,ex,ey,ez;
517
518 Point3i b,e;
519 int testposii,testposjj;
520 for(i=0;i<movvert.size();++i)
521 {
522 if(u.bbox.IsIn(movvert[i])){
523 u.PToIP(movvert[i],ip);
524 b=ip+Point3i(-range,-range,-range);
525 e=ip+Point3i( range, range, range);
526 while(b[0]<0) b[0]+=step;
527 while(e[0]>=u.siz[0]) e[0]-=step;
528 while(b[1]<0) b[1]+=step;
529 while(e[1]>=u.siz[1]) e[1]-=step;
530 while(b[2]<0) b[2]+=step;
531 while(e[2]>=u.siz[2]) e[2]-=step;
532
533 for(ii=b[0];ii<=e[0];ii+=step)
534 {
535 testposii=(ii-ip[0]+range)*wide2;
536 for(jj=b[1];jj<=e[1];jj+=step)
537 {
538 testposjj=testposii+(jj-ip[1]+range)*wide1-ip[2]+range;
539 for(kk=b[2];kk<=e[2];kk+=step)
540 {
541 UGrid< A2Mesh::face_container >::UG ** g = u.Grid(ii,jj,kk);
542 //if((*(g+1))- (*g) >0)
543 if((*(g+1))!=(*g) )
544 ++test[testposjj+kk];
545 assert(ii >=0 && ii < u.siz[0]);
546 assert(jj >=0 && jj < u.siz[1]);
547 assert(kk >=0 && kk < u.siz[2]);
548 }
549 }
550 }
551 }
552 }
553 int maxfnd=0;
554 Point3i BestI;
555 for(ii=-range;ii<=range;ii+=step)
556 for(jj=-range;jj<=range;jj+=step)
557 for(kk=-range;kk<=range;kk+=step)
558 {const int pos =(range+ii)*wide2+(range+jj)*wide1+range+kk;
559 if(test[pos]>maxfnd)
560 {
561 BestI=Point3i(ii,jj,kk);
562 BestTransV=Point3d(ii*u.voxel[0], jj*u.voxel[1], kk*u.voxel[2]);
563 maxfnd=test[pos];
564 }
565 //printf("Found %i su %i in %i\n",test[testcnt],movvert.size(),t1-t0);
566 }
567
568 int t1=clock();
569 if(Verbose) printf("BestTransl (%4i in %3ims) is %8.4f %8.4f %8.4f (%3i %3i %3i)\n",maxfnd,t1-t0,BestTransV[0],BestTransV[1],BestTransV[2],BestI[0],BestI[1],BestI[2]);
570 return maxfnd;
571 }
572 #if 0
573 int AlignPair::SearchTranslate(UGrid< A2Mesh::face_container > &u,
574 vector< Point3d > &movvert,
575 vector< Point3d > &movnorm,
576 Box3d &movbox,
577 const Matrix44d &BaseRot,
578 const int range,
579 const int step,
580 Point3d &BestTransV, // miglior vettore di spostamento
581 bool Verbose)
582 {
583 const int wide1=(range*2+1);
584 const int wide2=wide1*wide1;
585
586 int t0=clock();
587 InitMov(movvert,movnorm,movbox,BaseRot);
588 Point3i ip;
589 int i,ii,jj,kk;
590 vector<int> test((range*2+1)*(range*2+1)*(range*2+1),0);
591 for(i=0;i<movvert.size();++i)
592 {
593 if(u.bbox.IsIn(movvert[i])){
594 u.PToIP(movvert[i],ip);
595 for(ii=-range;ii<=range;ii+=step) if(ip[0]+ii>=0 && ip[0]+ii<u.siz[0])
596 for(jj=-range;jj<=range;jj+=step) if(ip[1]+jj>=0 && ip[1]+jj<u.siz[1])
597 for(kk=-range;kk<=range;kk+=step) if(ip[2]+kk>=0 && ip[2]+kk<u.siz[2])
598 {
599 UGrid< A2Mesh::face_container >::UG ** g = u.Grid(ip[0]+ii,ip[1]+jj,ip[2]+kk);
600 if((*(g+1))- (*g) >0)
601 ++test[(range+ii)*wide2+(range+jj)*wide1+range+kk];
602 }
603 }
604 }
605 int maxfnd=0;
606 Point3i BestI;
607 for(ii=-range;ii<=range;ii+=step)
608 for(jj=-range;jj<=range;jj+=step)
609 for(kk=-range;kk<=range;kk+=step)
610 {const int pos =(range+ii)*wide2+(range+jj)*wide1+range+kk;
611 assert(test2[pos]==test[pos]);
612 if(test[pos]>maxfnd)
613 {
614 BestI=Point3i(ii,jj,kk);
615 BestTransV=Point3d(ii*u.voxel[0], jj*u.voxel[1], kk*u.voxel[2]);
616 maxfnd=test[pos];
617 }
618 //printf("Found %i su %i in %i\n",test[testcnt],movvert.size(),t1-t0);
619 }
620
621 int t1=clock();
622 if(Verbose) printf("BestTransl (%4i in %3ims) is %8.4f %8.4f %8.4f (%3i %3i %3i)\n",maxfnd,t1-t0,BestTransV[0],BestTransV[1],BestTransV[2],BestI[0],BestI[1],BestI[2]);
623 return maxfnd;
624 }
625 */
626
627
628 /**********************************************************/
629 // Funzioni per la scelta dei vertici sulla mesh da muovere
630
SampleMovVert(vector<A2Vertex> & vert,int SampleNum,AlignPair::Param::SampleModeEnum SampleMode)631 bool AlignPair::SampleMovVert(vector<A2Vertex> &vert, int SampleNum, AlignPair::Param::SampleModeEnum SampleMode)
632 {
633 switch (SampleMode)
634 {
635 case AlignPair::Param::SMRandom: return SampleMovVertRandom(vert, SampleNum);
636 case AlignPair::Param::SMNormalEqualized: return SampleMovVertNormalEqualized(vert, SampleNum);
637 default: assert(0);
638 }
639 return false;
640 }
641
642
643 // Scelta a caso semplice
SampleMovVertRandom(vector<A2Vertex> & vert,int SampleNum)644 bool AlignPair::SampleMovVertRandom(vector<A2Vertex> &vert, int SampleNum)
645 {
646 if (int(vert.size()) <= SampleNum) return true;
647 int i;
648 for (i = 0; i < SampleNum; ++i)
649 {
650 int pos = myrnd.generate(vert.size());
651 assert(pos >= 0 && pos < int(vert.size()));
652 swap(vert[i], vert[pos]);
653 }
654 vert.resize(SampleNum);
655 return true;
656 }
657
658 /*
659 Scelta a caso in maniera tale che la distribuzione delle normali dei
660 punti scelti sia il piu' possibile uniforme. In questo modo anche piccole
661 parti inclinate vengono sicuramente campionate
662
663 Si precalcola un piccolo (42) insieme di normali e si fa bucketing di
664 tutti i vertici della mesh su di esse.
665 Poi si scelgono <SampleNum> punti scegliendo ogni volta prima un bucket
666 e poi un punto all'interno del bucket
667
668 */
669
SampleMovVertNormalEqualized(vector<A2Vertex> & vert,int SampleNum)670 bool AlignPair::SampleMovVertNormalEqualized(vector<A2Vertex> &vert, int SampleNum)
671 {
672 // assert(0);
673
674 // int t0=clock();
675 vector<Point3d> NV;
676 if (NV.size() == 0)
677 {
678 GenNormal<double>::Fibonacci(30, NV);
679 printf("Generated %i normals\n", int(NV.size()));
680 }
681 // Bucket vector dove, per ogni normale metto gli indici
682 // dei vertici ad essa corrispondenti
683 vector<vector <int> > BKT(NV.size());
684 for (size_t i = 0; i < vert.size(); ++i)
685 {
686 int ind = GenNormal<double>::BestMatchingNormal(vert[i].N(), NV);
687 BKT[ind].push_back(int(i));
688 }
689 //int t1=clock();
690
691 // vettore di contatori per sapere quanti punti ho gia' preso per ogni bucket
692 vector <int> BKTpos(BKT.size(), 0);
693
694 if (SampleNum >= int(vert.size())) SampleNum = vert.size() - 1;
695
696 for (int i = 0; i < SampleNum;)
697 {
698 int ind = myrnd.generate(BKT.size()); // Scelgo un Bucket
699 int &CURpos = BKTpos[ind];
700 vector<int> &CUR = BKT[ind];
701
702 if (CURpos<int(CUR.size()))
703 {
704 swap(CUR[CURpos], CUR[CURpos + myrnd.generate(BKT[ind].size() - CURpos)]);
705 swap(vert[i], vert[CUR[CURpos]]);
706 ++BKTpos[ind];
707 ++i;
708 }
709 }
710 vert.resize(SampleNum);
711 // int t2=clock();
712 // printf("Matching %6i\n",t1-t0);
713 // printf("Collecting %6i\n",t2-t1);
714 // printf("Total %6i\n",t2-t0);
715
716 return true;
717 }
718
719
720