00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 #ifndef __VCGLIB__SAMPLING
00100 #define __VCGLIB__SAMPLING
00101
00102 #include <time.h>
00103
00104 #include <vcg/complex/trimesh/closest.h>
00105 #include <vcg/space/box3.h>
00106 #include <vcg/math/histogram.h>
00107 #include <vcg/space/color4.h>
00108 #include <vcg/simplex/face/distance.h>
00109 #include <vcg/complex/trimesh/update/color.h>
00110 #include <vcg/space/index/grid_static_ptr.h>
00111 #include <vcg/space/index/aabb_binary_tree/aabb_binary_tree.h>
00112 #include <vcg/space/index/octree.h>
00113 #include <vcg/space/index/spatial_hashing.h>
00114 namespace vcg
00115 {
00116
00117 struct SamplingFlags{
00118 enum{
00119 HIST = 0x0001,
00120 VERTEX_SAMPLING = 0x0002,
00121 EDGE_SAMPLING = 0x0004,
00122 FACE_SAMPLING = 0x0008,
00123 MONTECARLO_SAMPLING = 0x0010,
00124 SUBDIVISION_SAMPLING = 0x0020,
00125 SIMILAR_SAMPLING = 0x0040,
00126 NO_SAMPLING = 0x0070,
00127 SAVE_ERROR = 0x0100,
00128 INCLUDE_UNREFERENCED_VERTICES = 0x0200,
00129 USE_STATIC_GRID = 0x0400,
00130 USE_HASH_GRID = 0x0800,
00131 USE_AABB_TREE = 0x1000,
00132 USE_OCTREE = 0x2000
00133 };
00134 };
00135
00136 template <class MetroMesh>
00137 class Sampling
00138 {
00139 public:
00140
00141 private:
00142 typedef typename MetroMesh::CoordType CoordType;
00143 typedef typename MetroMesh::ScalarType ScalarType;
00144 typedef typename MetroMesh::VertexType VertexType;
00145 typedef typename MetroMesh::VertexPointer VertexPointer;
00146 typedef typename MetroMesh::VertexIterator VertexIterator;
00147 typedef typename MetroMesh::FaceIterator FaceIterator;
00148 typedef typename MetroMesh::FaceType FaceType;
00149 typedef typename MetroMesh::FaceContainer FaceContainer;
00150
00151 typedef GridStaticPtr <FaceType, typename MetroMesh::ScalarType > MetroMeshGrid;
00152 typedef SpatialHashTable <FaceType, typename MetroMesh::ScalarType > MetroMeshHash;
00153 typedef AABBBinaryTreeIndex <FaceType, typename MetroMesh::ScalarType, vcg::EmptyClass> MetroMeshAABB;
00154 typedef Octree <FaceType, typename MetroMesh::ScalarType > MetroMeshOctree;
00155
00156 typedef Point3<typename MetroMesh::ScalarType> Point3x;
00157
00158
00159
00160
00161
00162 MetroMesh &S1;
00163 MetroMesh &S2;
00164 MetroMeshGrid gS2;
00165 MetroMeshHash hS2;
00166 MetroMeshAABB tS2;
00167 MetroMeshOctree oS2;
00168
00169
00170 unsigned int n_samples_per_face ;
00171 float n_samples_edge_to_face_ratio ;
00172 float bbox_factor ;
00173 float inflate_percentage ;
00174 unsigned int min_size ;
00175 int n_hist_bins ;
00176 int print_every_n_elements ;
00177 int referredBit ;
00178
00179 double dist_upper_bound;
00180 double n_samples_per_area_unit;
00181 unsigned long n_samples_target;
00182 int Flags;
00183
00184
00185 Histogram<double> hist;
00186 unsigned long n_total_samples;
00187 unsigned long n_total_area_samples;
00188 unsigned long n_total_edge_samples;
00189 unsigned long n_total_vertex_samples;
00190 double max_dist;
00191 double mean_dist;
00192 double RMS_dist;
00193 double volume;
00194 double area_S1;
00195
00196
00197 int n_samples;
00198
00199
00200 inline double ComputeMeshArea(MetroMesh & mesh);
00201 float AddSample(const Point3x &p);
00202 inline void AddRandomSample(FaceIterator &T);
00203 inline void SampleEdge(const Point3x & v0, const Point3x & v1, int n_samples_per_edge);
00204 void VertexSampling();
00205 void EdgeSampling();
00206 void FaceSubdiv(const Point3x & v0, const Point3x &v1, const Point3x & v2, int maxdepth);
00207 void SimilarTriangles(const Point3x &v0, const Point3x &v1, const Point3x &v2, int n_samples_per_edge);
00208 void MontecarloFaceSampling();
00209 void SubdivFaceSampling();
00210 void SimilarFaceSampling();
00211
00212 public :
00213
00214 Sampling(MetroMesh &_s1, MetroMesh &_s2);
00215 ~Sampling();
00216 void Hausdorff();
00217 double GetArea() {return area_S1;}
00218 double GetDistMax() {return max_dist;}
00219 double GetDistMean() {return mean_dist;}
00220 double GetDistRMS() {return RMS_dist;}
00221 double GetDistVolume() {return volume;}
00222 unsigned long GetNSamples() {return n_total_samples;}
00223 unsigned long GetNAreaSamples() {return n_total_area_samples;}
00224 unsigned long GetNEdgeSamples() {return n_total_edge_samples;}
00225 unsigned long GetNVertexSamples() {return n_total_vertex_samples;}
00226 double GetNSamplesPerAreaUnit() {return n_samples_per_area_unit;}
00227 unsigned long GetNSamplesTarget() {return n_samples_target;}
00228 Histogram<double> &GetHist() {return hist;}
00229 void SetFlags(int flags) {Flags = flags;}
00230 void ClearFlag(int flag) {Flags &= (flag ^ -1);}
00231 void SetParam(double _n_samp) {n_samples_target = _n_samp;}
00232 void SetSamplesTarget(unsigned long _n_samp);
00233 void SetSamplesPerAreaUnit(double _n_samp);
00234 };
00235
00236
00237
00238
00239 template <class MetroMesh>
00240 Sampling<MetroMesh>::Sampling(MetroMesh &_s1, MetroMesh &_s2):S1(_s1),S2(_s2)
00241 {
00242 Flags = 0;
00243 area_S1 = ComputeMeshArea(_s1);
00244
00245 n_samples_per_face = 10;
00246 n_samples_edge_to_face_ratio = 0.1f;
00247 bbox_factor = 0.1f;
00248 inflate_percentage = 0.02f;
00249 min_size = 125;
00250 n_hist_bins = 256;
00251 print_every_n_elements = S1.fn/100;
00252
00253 if(print_every_n_elements <= 1)
00254 print_every_n_elements = 2;
00255
00256 referredBit = VertexType::NewBitFlag();
00257
00258 FaceIterator fi; VertexIterator vi; int i;
00259 for(fi = _s1.face.begin(); fi!= _s1.face.end(); ++fi)
00260 for(i=0;i<3;++i) (*fi).V(i)->SetUserBit(referredBit);
00261 }
00262
00263 template <class MetroMesh>
00264 Sampling<MetroMesh>::~Sampling()
00265 {
00266 VertexType::DeleteBitFlag(referredBit);
00267 }
00268
00269
00270
00271 template <class MetroMesh>
00272 void Sampling<MetroMesh>::SetSamplesTarget(unsigned long _n_samp)
00273 {
00274 n_samples_target = _n_samp;
00275 n_samples_per_area_unit = n_samples_target / (double)area_S1;
00276 }
00277
00278 template <class MetroMesh>
00279 void Sampling<MetroMesh>::SetSamplesPerAreaUnit(double _n_samp)
00280 {
00281 n_samples_per_area_unit = _n_samp;
00282 n_samples_target = (unsigned long)((double) n_samples_per_area_unit * area_S1);
00283 }
00284
00285
00286
00287 template <class MetroMesh>
00288 inline double Sampling<MetroMesh>::ComputeMeshArea(MetroMesh & mesh)
00289 {
00290 FaceIterator face;
00291 double area = 0.0;
00292
00293 for(face=mesh.face.begin(); face != mesh.face.end(); face++)
00294 if(!(*face).IsD())
00295 area += DoubleArea(*face);
00296
00297 return area/2.0;
00298 }
00299
00300 template <class MetroMesh>
00301 float Sampling<MetroMesh>::AddSample(const Point3x &p )
00302 {
00303 FaceType *f=0;
00304 Point3x normf, bestq, ip;
00305 ScalarType dist;
00306
00307 dist = dist_upper_bound;
00308
00309
00310 if(Flags & SamplingFlags::USE_AABB_TREE)
00311 f=tri::GetClosestFace<MetroMesh,MetroMeshAABB>(S2, tS2, p, dist_upper_bound, dist, normf, bestq, ip);
00312 if(Flags & SamplingFlags::USE_HASH_GRID)
00313 f=tri::GetClosestFace<MetroMesh,MetroMeshHash>(S2, hS2, p, dist_upper_bound, dist, normf, bestq, ip);
00314 if(Flags & SamplingFlags::USE_STATIC_GRID)
00315 f=tri::GetClosestFace<MetroMesh,MetroMeshGrid>(S2, gS2, p, dist_upper_bound, dist, normf, bestq, ip);
00316 if (Flags & SamplingFlags::USE_OCTREE)
00317 f=tri::GetClosestFace<MetroMesh,MetroMeshOctree>(S2, oS2, p, dist_upper_bound, dist, normf, bestq, ip);
00318
00319
00320 if(dist == dist_upper_bound)
00321 return -1.0;
00322
00323 if(dist > max_dist)
00324 max_dist = dist;
00325 mean_dist += dist;
00326 RMS_dist += dist*dist;
00327 n_total_samples++;
00328
00329 if(Flags & SamplingFlags::HIST)
00330 hist.Add((float)fabs(dist));
00331
00332 return (float)dist;
00333 }
00334
00335
00336
00337
00338
00339 template <class MetroMesh>
00340 void Sampling<MetroMesh>::VertexSampling()
00341 {
00342
00343 int cnt = 0;
00344 float error;
00345
00346 printf("Vertex sampling\n");
00347 VertexIterator vi;
00348 typename std::vector<VertexPointer>::iterator vif;
00349 for(vi=S1.vert.begin();vi!=S1.vert.end();++vi)
00350 if( (*vi).IsUserBit(referredBit) ||
00351 ((Flags&SamplingFlags::INCLUDE_UNREFERENCED_VERTICES) != 0) )
00352 {
00353 error = AddSample((*vi).cP());
00354
00355 n_total_vertex_samples++;
00356
00357
00358 if(Flags & SamplingFlags::SAVE_ERROR) (*vi).Q() = error;
00359
00360
00361 if(!(++cnt % print_every_n_elements))
00362 printf("Sampling vertices %d%%\r", (100 * cnt/S1.vn));
00363 }
00364 printf(" \r");
00365 }
00366
00367
00368
00369
00370
00371 template <class MetroMesh>
00372 inline void Sampling<MetroMesh>::SampleEdge(const Point3x & v0, const Point3x & v1, int n_samples_per_edge)
00373 {
00374
00375 Point3x e((v1-v0)/(double)(n_samples_per_edge+1));
00376 int i;
00377
00378 for(i=1; i <= n_samples_per_edge; i++)
00379 {
00380 AddSample(v0 + e*i);
00381 n_total_edge_samples++;
00382 }
00383 }
00384
00385
00386 template <class MetroMesh>
00387 void Sampling<MetroMesh>::EdgeSampling()
00388 {
00389
00390 typedef std::pair<VertexPointer, VertexPointer> pvv;
00391 std::vector< pvv > Edges;
00392
00393 printf("Edge sampling\n");
00394
00395
00396 FaceIterator fi;
00397 for(fi=S1.face.begin(); fi != S1.face.end(); fi++)
00398 for(int i=0; i<3; ++i)
00399 {
00400 Edges.push_back(make_pair((*fi).V0(i),(*fi).V1(i)));
00401 if(Edges.back().first > Edges.back().second)
00402 swap(Edges.back().first, Edges.back().second);
00403 }
00404 sort(Edges.begin(), Edges.end());
00405 typename std::vector< pvv>::iterator edgeend = unique(Edges.begin(), Edges.end());
00406 Edges.resize(edgeend-Edges.begin());
00407
00408
00409 typename std::vector<pvv>::iterator ei;
00410 double n_samples_per_length_unit;
00411 double n_samples_decimal = 0.0;
00412 int cnt=0;
00413 if(Flags & SamplingFlags::FACE_SAMPLING)
00414 n_samples_per_length_unit = sqrt((double)n_samples_per_area_unit);
00415 else
00416 n_samples_per_length_unit = n_samples_per_area_unit;
00417 for(ei=Edges.begin(); ei!=Edges.end(); ++ei)
00418 {
00419 n_samples_decimal += Distance((*ei).first->cP(),(*ei).second->cP()) * n_samples_per_length_unit;
00420 n_samples = (int) n_samples_decimal;
00421 SampleEdge((*ei).first->cP(), (*ei).second->cP(), (int) n_samples);
00422 n_samples_decimal -= (double) n_samples;
00423
00424
00425 if(!(++cnt % print_every_n_elements))
00426 printf("Sampling edge %d%%\r", (100 * cnt/Edges.size()));
00427 }
00428 printf(" \r");
00429 }
00430
00431
00432
00433
00434
00435
00436 template <class MetroMesh>
00437 inline void Sampling<MetroMesh>::AddRandomSample(FaceIterator &T)
00438 {
00439
00440 double rnd_1, rnd_2;
00441
00442
00443 Point3x p0(T->V(0)->cP());
00444 Point3x p1(T->V(1)->cP());
00445 Point3x p2(T->V(2)->cP());
00446
00447 Point3x v1(p1 - p0);
00448 Point3x v2(p2 - p0);
00449
00450
00451 rnd_1 = (double)rand() / (double)RAND_MAX;
00452 rnd_2 = (double)rand() / (double)RAND_MAX;
00453 if(rnd_1 + rnd_2 > 1.0)
00454 {
00455 rnd_1 = 1.0 - rnd_1;
00456 rnd_2 = 1.0 - rnd_2;
00457 }
00458
00459
00460 AddSample (p0 + (v1 * rnd_1 + v2 * rnd_2));
00461 n_total_area_samples++;
00462 }
00463
00464 template <class MetroMesh>
00465 void Sampling<MetroMesh>::MontecarloFaceSampling()
00466 {
00467
00468 int cnt = 0;
00469 double n_samples_decimal = 0.0;
00470 FaceIterator fi;
00471
00472 srand(clock());
00473
00474 for(fi=S1.face.begin(); fi != S1.face.end(); fi++)
00475 if(!(*fi).IsD())
00476 {
00477
00478 n_samples_decimal += 0.5*DoubleArea(*fi) * n_samples_per_area_unit;
00479 n_samples = (int) n_samples_decimal;
00480
00481
00482 for(int i=0; i < n_samples; i++)
00483 AddRandomSample(fi);
00484
00485 n_samples_decimal -= (double) n_samples;
00486
00487
00488
00489
00490 }
00491
00492 }
00493
00494
00495
00496 template <class MetroMesh>
00497 void Sampling<MetroMesh>::FaceSubdiv(const Point3x & v0, const Point3x & v1, const Point3x & v2, int maxdepth)
00498 {
00499
00500 if(maxdepth == 0)
00501 {
00502
00503 AddSample((v0+v1+v2)/3.0f);
00504 n_total_area_samples++;
00505 n_samples++;
00506 return;
00507 }
00508
00509
00510 double maxd01 = SquaredDistance(v0,v1);
00511 double maxd12 = SquaredDistance(v1,v2);
00512 double maxd20 = SquaredDistance(v2,v0);
00513 int res;
00514 if(maxd01 > maxd12)
00515 if(maxd01 > maxd20) res = 0;
00516 else res = 2;
00517 else
00518 if(maxd12 > maxd20) res = 1;
00519 else res = 2;
00520
00521
00522 Point3x pp;
00523 switch(res)
00524 {
00525 case 0 : pp = (v0+v1)/2;
00526 FaceSubdiv(v0,pp,v2,maxdepth-1);
00527 FaceSubdiv(pp,v1,v2,maxdepth-1);
00528 break;
00529 case 1 : pp = (v1+v2)/2;
00530 FaceSubdiv(v0,v1,pp,maxdepth-1);
00531 FaceSubdiv(v0,pp,v2,maxdepth-1);
00532 break;
00533 case 2 : pp = (v2+v0)/2;
00534 FaceSubdiv(v0,v1,pp,maxdepth-1);
00535 FaceSubdiv(pp,v1,v2,maxdepth-1);
00536 break;
00537 }
00538 }
00539
00540 template <class MetroMesh>
00541 void Sampling<MetroMesh>::SubdivFaceSampling()
00542 {
00543
00544 int cnt = 0, maxdepth;
00545 double n_samples_decimal = 0.0;
00546 typename MetroMesh::FaceIterator fi;
00547
00548 printf("Subdivision face sampling\n");
00549 for(fi=S1.face.begin(); fi != S1.face.end(); fi++)
00550 {
00551
00552 n_samples_decimal += 0.5*DoubleArea(*fi) * n_samples_per_area_unit;
00553 n_samples = (int) n_samples_decimal;
00554 if(n_samples)
00555 {
00556
00557 maxdepth = ((int)(log((double)n_samples)/log(2.0)));
00558 n_samples = 0;
00559 FaceSubdiv((*fi).V(0)->cP(), (*fi).V(1)->cP(), (*fi).V(2)->cP(), maxdepth);
00560 }
00561 n_samples_decimal -= (double) n_samples;
00562
00563
00564 if(!(++cnt % print_every_n_elements))
00565 printf("Sampling face %d%%\r", (100 * cnt/S1.fn));
00566 }
00567 printf(" \r");
00568 }
00569
00570
00571
00572 template <class MetroMesh>
00573 void Sampling<MetroMesh>::SimilarTriangles(const Point3x & v0, const Point3x & v1, const Point3x & v2, int n_samples_per_edge)
00574 {
00575 Point3x V1((v1-v0)/(double)(n_samples_per_edge-1));
00576 Point3x V2((v2-v0)/(double)(n_samples_per_edge-1));
00577 int i, j;
00578
00579
00580 for(i=1; i < n_samples_per_edge-1; i++)
00581 for(j=1; j < n_samples_per_edge-1-i; j++)
00582 {
00583 AddSample( v0 + (V1*(double)i + V2*(double)j) );
00584 n_total_area_samples++;
00585 n_samples++;
00586 }
00587 }
00588
00589 template <class MetroMesh>
00590 void Sampling<MetroMesh>::SimilarFaceSampling()
00591 {
00592
00593 int cnt = 0, n_samples_per_edge;
00594 double n_samples_decimal = 0.0;
00595 FaceIterator fi;
00596
00597 printf("Similar Triangles face sampling\n");
00598 for(fi=S1.face.begin(); fi != S1.face.end(); fi++)
00599 {
00600
00601 n_samples_decimal += 0.5*DoubleArea(*fi) * n_samples_per_area_unit;
00602 n_samples = (int) n_samples_decimal;
00603 if(n_samples)
00604 {
00605
00606 n_samples_per_edge = (int)((sqrt(1.0+8.0*(double)n_samples) +5.0)/2.0);
00607 n_samples = 0;
00608 SimilarTriangles((*fi).V(0)->cP(), (*fi).V(1)->cP(), (*fi).V(2)->cP(), n_samples_per_edge);
00609 }
00610 n_samples_decimal -= (double) n_samples;
00611
00612
00613 if(!(++cnt % print_every_n_elements))
00614 printf("Sampling face %d%%\r", (100 * cnt/S1.fn));
00615 }
00616 printf(" \r");
00617 }
00618
00619
00620
00621
00622
00623 template <class MetroMesh>
00624 void Sampling<MetroMesh>::Hausdorff()
00625 {
00626 Box3< ScalarType> bbox;
00627
00628 typedef typename std::vector<FaceType>::iterator FaceVecIterator;
00629
00630 if(Flags & SamplingFlags::USE_HASH_GRID) hS2.Set(S2.face.begin(),S2.face.end());
00631 if(Flags & SamplingFlags::USE_AABB_TREE) tS2.Set(S2.face.begin(),S2.face.end());
00632 if(Flags & SamplingFlags::USE_STATIC_GRID) gS2.Set(S2.face.begin(),S2.face.end());
00633 if(Flags & SamplingFlags::USE_OCTREE) oS2.Set(S2.face.begin(),S2.face.end());
00634
00635
00636 bbox = S2.bbox;
00637 dist_upper_bound = bbox.Diag();
00638 if(Flags & SamplingFlags::HIST)
00639 hist.SetRange(0.0, dist_upper_bound/100.0, n_hist_bins);
00640
00641
00642 n_total_area_samples = n_total_edge_samples = n_total_vertex_samples = n_total_samples = n_samples = 0;
00643 max_dist = -HUGE_VAL;
00644 mean_dist = RMS_dist = 0;
00645
00646
00647 if(Flags & SamplingFlags::VERTEX_SAMPLING)
00648 VertexSampling();
00649
00650 if(n_samples_target > n_total_samples)
00651 {
00652 n_samples_target -= (int) n_total_samples;
00653 n_samples_per_area_unit = n_samples_target / area_S1;
00654 if(Flags & SamplingFlags::EDGE_SAMPLING)
00655 {
00656 EdgeSampling();
00657 if(n_samples_target > n_total_samples) n_samples_target -= (int) n_total_samples;
00658 else n_samples_target=0;
00659 }
00660
00661 if((Flags & SamplingFlags::FACE_SAMPLING) && (n_samples_target > 0))
00662 {
00663 n_samples_per_area_unit = n_samples_target / area_S1;
00664 if(Flags & SamplingFlags::MONTECARLO_SAMPLING) MontecarloFaceSampling();
00665 if(Flags & SamplingFlags::SUBDIVISION_SAMPLING) SubdivFaceSampling();
00666 if(Flags & SamplingFlags::SIMILAR_SAMPLING) SimilarFaceSampling();
00667 }
00668 }
00669
00670
00671 if(Flags & SamplingFlags::SAVE_ERROR)
00672 vcg::tri::UpdateColor<MetroMesh>::VertexQualityRamp(S1);
00673
00674
00675 n_samples_per_area_unit = (double) n_total_samples / area_S1;
00676 volume = mean_dist / n_samples_per_area_unit / 2.0;
00677 mean_dist /= n_total_samples;
00678 RMS_dist = sqrt(RMS_dist / n_total_samples);
00679 }
00680 }
00681 #endif