VCG Library
curvature_fitting.h
1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004-2016 \/)\/ *
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 
25 ****************************************************************************/
26 
27 #ifndef VCGLIB_UPDATE_CURVATURE_FITTING
28 #define VCGLIB_UPDATE_CURVATURE_FITTING
29 
30 #include <vcg/space/index/grid_static_ptr.h>
31 #include <vcg/simplex/face/topology.h>
32 #include <vcg/simplex/face/pos.h>
33 #include <vcg/simplex/face/jumping_pos.h>
34 #include <vcg/container/simple_temporary_data.h>
35 #include <vcg/complex/algorithms/update/normal.h>
36 #include <vcg/complex/algorithms/point_sampling.h>
37 #include <vcg/complex/algorithms/intersection.h>
38 #include <vcg/complex/algorithms/inertia.h>
39 #include <vcg/complex/algorithms/nring.h>
40 
41 #include <Eigen/Core>
42 #include <Eigen/QR>
43 #include <Eigen/LU>
44 #include <Eigen/SVD>
45 #include <Eigen/Eigenvalues>
46 
47 
48 namespace vcg {
49 namespace tri {
50 
52 
54 
56 
60 template <class MeshType>
62 {
63 
64 public:
65  typedef typename MeshType::FaceType FaceType;
66  typedef typename MeshType::FacePointer FacePointer;
67  typedef typename MeshType::FaceIterator FaceIterator;
68  typedef typename MeshType::VertexIterator VertexIterator;
69  typedef typename MeshType::VertContainer VertContainer;
70  typedef typename MeshType::VertexType VertexType;
71  typedef typename MeshType::VertexPointer VertexPointer;
72  typedef typename MeshType::VertexPointer VertexTypeP;
73  typedef vcg::face::VFIterator<FaceType> VFIteratorType;
74  typedef typename MeshType::CoordType CoordType;
75  typedef typename CoordType::ScalarType ScalarType;
76 
77 class Quadric
78 {
79  public:
80 
81  Quadric(double av, double bv, double cv, double dv, double ev)
82  {
83  a() = av;
84  b() = bv;
85  c() = cv;
86  d() = dv;
87  e() = ev;
88  }
89 
90  double& a() { return data[0];}
91  double& b() { return data[1];}
92  double& c() { return data[2];}
93  double& d() { return data[3];}
94  double& e() { return data[4];}
95 
96  double data[5];
97 
98  double evaluate(double u, double v)
99  {
100  return a()*u*u + b()*u*v + c()*v*v + d()*u + e()*v;
101  }
102 
103  double du(double u, double v)
104  {
105  return 2.0*a()*u + b()*v + d();
106  }
107 
108  double dv(double u, double v)
109  {
110  return 2.0*c()*v + b()*u + e();
111  }
112 
113  double duv(double /*u*/, double /*v*/)
114  {
115  return b();
116  }
117 
118  double duu(double /*u*/, double /*v*/)
119  {
120  return 2.0*a();
121  }
122 
123  double dvv(double /*u*/, double /*v*/)
124  {
125  return 2.0*c();
126  }
127 
128  static Quadric fit(std::vector<CoordType> VV)
129  {
130  assert(VV.size() >= 5);
131  Eigen::MatrixXf A(VV.size(),5);
132  Eigen::MatrixXf b(VV.size(),1);
133  Eigen::MatrixXf sol(VV.size(),1);
134 
135  for(unsigned int c=0; c < VV.size(); ++c)
136  {
137  double u = VV[c].X();
138  double v = VV[c].Y();
139  double n = VV[c].Z();
140 
141  A(c,0) = u*u;
142  A(c,1) = u*v;
143  A(c,2) = v*v;
144  A(c,3) = u;
145  A(c,4) = v;
146 
147  b(c,0) = n;
148  }
149 
150  sol = ((A.transpose()*A).inverse()*A.transpose())*b;
151  return Quadric(sol(0,0),sol(1,0),sol(2,0),sol(3,0),sol(4,0));
152  }
153 };
154 
155  static CoordType project(VertexType* v, VertexType* vp)
156  {
157  return vp->P() - (v->N() * ((vp->P() - v->P()) * v->N()));
158  }
159 
160 
161  static std::vector<CoordType> computeReferenceFrames(VertexTypeP vi)
162  {
163  vcg::face::VFIterator<FaceType> vfi(vi);
164 
165  int i = (vfi.I()+1)%3;
166  VertexTypeP vp = vfi.F()->V(i);
167 
168  CoordType x = (project(&*vi,vp) - vi->P()).Normalize();
169  //assert(fabs(x * vi->N()) < 0.1);
170 
171  std::vector<CoordType> res(3);
172 
173  res[0] = x;
174  res[1] = (vi->N() ^ res[0]).Normalize();
175  res[2] = (vi->N())/(vi->N()).Norm();
176 
177  return res;
178  }
179 
180  static std::set<CoordType> getSecondRing(VertexTypeP v)
181  {
182  std::set<VertexTypeP> ris;
183  std::set<CoordType> coords;
184 
185  vcg::face::VFIterator<FaceType> vvi(v);
186  for(;!vvi.End();++vvi)
187  {
188  vcg::face::VFIterator<FaceType> vvi2(vvi.F()->V((vvi.I()+1)%3));
189  for(;!vvi2.End();++vvi2)
190  {
191  ris.insert(vvi2.F()->V((vvi2.I()+1)%3));
192  }
193  }
194  typename std::set<VertexTypeP>::iterator it;
195  for(it = ris.begin(); it != ris.end(); ++it)
196  coords.insert((*it)->P());
197 
198  return coords;
199  }
200 
201  static Quadric fitQuadric(VertexTypeP v, std::vector<CoordType>& ref)
202  {
203  std::set<CoordType> ring = getSecondRing(v);
204  if (ring.size() < 5)
205  return Quadric(1,1,1,1,1);
206  std::vector<CoordType> points;
207 
208  typename std::set<CoordType>::iterator b = ring.begin();
209  typename std::set<CoordType>::iterator e = ring.end();
210 
211  while(b != e)
212  {
213  // vtang non e` il v tangente!!!
214  CoordType vTang = *b - v->P();
215 
216  double x = vTang * ref[0];
217  double y = vTang * ref[1];
218  double z = vTang * ref[2];
219  points.push_back(CoordType(x,y,z));
220  ++b;
221  }
222  return Quadric::fit(points);
223  }
224 
225 
226  static void computeCurvature(MeshType & m)
227  {
229  tri::RequireCompactness(m);
230  tri::RequireVFAdjacency(m);
231 
233 
236 
237 
238  VertexIterator vi;
239  for(vi = m.vert.begin(); vi!=m.vert.end(); ++vi )
240  {
241  std::vector<CoordType> ref = computeReferenceFrames(&*vi);
242 
243  Quadric q = fitQuadric(&*vi,ref);
244  double a = q.a();
245  double b = q.b();
246  double c = q.c();
247  double d = q.d();
248  double e = q.e();
249 
250  double E = 1.0 + d*d;
251  double F = d*e;
252  double G = 1.0 + e*e;
253 
254  CoordType n = CoordType(-d,-e,1.0).Normalize();
255 
256  vi->N() = ref[0] * n[0] + ref[1] * n[1] + ref[2] * n[2];
257 
258  double L = 2.0 * a * n.Z();
259  double M = b * n.Z();
260  double N = 2 * c * n.Z();
261 
262  // ----------------- Eigen stuff
263  Eigen::Matrix2d m;
264  m << L*G - M*F, M*E-L*F, M*E-L*F, N*E-M*F;
265  m = m / (E*G-F*F);
266  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig(m);
267 
268  Eigen::Vector2d c_val = eig.eigenvalues();
269  Eigen::Matrix2d c_vec = eig.eigenvectors();
270 
271  c_val = -c_val;
272 
273  CoordType v1, v2;
274  v1[0] = c_vec(0,0);
275  v1[1] = c_vec(0,1);
276  v1[2] = 0;
277 
278  v2[0] = c_vec(1,0);
279  v2[1] = c_vec(1,1);
280  v2[2] = 0;
281 
282  v1 = v1.Normalize();
283  v2 = v2.Normalize();
284 
285  v1 = v1 * c_val[0];
286  v2 = v2 * c_val[1];
287 
288  CoordType v1global = ref[0] * v1[0] + ref[1] * v1[1] + ref[2] * v1[2];
289  CoordType v2global = ref[0] * v2[0] + ref[1] * v2[1] + ref[2] * v2[2];
290 
291  v1global.Normalize();
292  v2global.Normalize();
293 
294  if (c_val[0] > c_val[1])
295  {
296  (*vi).PD1().Import(v1global);
297  (*vi).PD2().Import(v2global);
298  (*vi).K1() = c_val[0];
299  (*vi).K2() = c_val[1];
300  }
301  else
302  {
303  (*vi).PD1().Import(v2global);
304  (*vi).PD2().Import(v1global);
305  (*vi).K1() = c_val[1];
306  (*vi).K2() = c_val[0];
307  }
308  // ---- end Eigen stuff
309  }
310  }
311 
312  // GG LOCAL CURVATURE
313 
314  class QuadricLocal
315  {
316  public:
317 
318  QuadricLocal ()
319  {
320  a() = b() = c() = d() = e() = 1.0;
321  }
322 
323  QuadricLocal (double av, double bv, double cv, double dv, double ev)
324  {
325  a() = av;
326  b() = bv;
327  c() = cv;
328  d() = dv;
329  e() = ev;
330  }
331 
332  double& a() { return data[0];}
333  double& b() { return data[1];}
334  double& c() { return data[2];}
335  double& d() { return data[3];}
336  double& e() { return data[4];}
337 
338  double data[5];
339 
340  double evaluate(double u, double v)
341  {
342  return a()*u*u + b()*u*v + c()*v*v + d()*u + e()*v;
343  }
344 
345  double du(double u, double v)
346  {
347  return 2.0*a()*u + b()*v + d();
348  }
349 
350  double dv(double u, double v)
351  {
352  return 2.0*c()*v + b()*u + e();
353  }
354 
355  double duv(double /*u*/, double /*v*/)
356  {
357  return b();
358  }
359 
360  double duu(double /*u*/, double /*v*/)
361  {
362  return 2.0*a();
363  }
364 
365  double dvv(double /*u*/, double /*v*/)
366  {
367  return 2.0*c();
368  }
369 
370 
371  static QuadricLocal fit(std::vector<CoordType> &VV, bool svdRes, bool detCheck)
372  {
373  assert(VV.size() >= 5);
374  Eigen::MatrixXd A(VV.size(),5);
375  Eigen::MatrixXd b(VV.size(),1);
376  Eigen::MatrixXd sol(5,1);
377 
378  for(unsigned int c=0; c < VV.size(); ++c)
379  {
380  double u = VV[c].X();
381  double v = VV[c].Y();
382  double n = VV[c].Z();
383 
384  A(c,0) = u*u;
385  A(c,1) = u*v;
386  A(c,2) = v*v;
387  A(c,3) = u;
388  A(c,4) = v;
389 
390  b[c] = n;
391  }
392 
393 
394  static int count = 0, index = 0;
395  double min = 0.000000000001; //1.0e-12
396  /*
397  if (!count)
398  printf("GNE %e\n", min);
399  */
400 
401  if (detCheck && ((A.transpose()*A).determinant() < min && (A.transpose()*A).determinant() > -min))
402  {
403  //A.svd().solve(b, &sol); A.svd().solve(b, &sol);
404  //cout << sol << endl;
405  printf("Quadric: unsolvable vertex %d %d\n", count, ++index);
406  //return Quadric (1, 1, 1, 1, 1);
407 // A.svd().solve(b, &sol);
408  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A);
409  sol=svd.solve(b);
410  return QuadricLocal(sol[0],sol[1],sol[2],sol[3],sol[4]);
411  }
412  count++;
413 
414  //for (int i = 0; i < 100; i++)
415  {
416  if (svdRes)
417  {
418  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A);
419  sol=svd.solve(b);
420  //A.svd().solve(b, &sol);
421  }
422  else
423  sol = ((A.transpose()*A).inverse()*A.transpose())*b;
424 
425  }
426 
427  return QuadricLocal(sol[0],sol[1],sol[2],sol[3],sol[4]);
428  }
429  };
430 
431  static void expandMaxLocal (MeshType & mesh, VertexType *v, int max, std::vector<VertexType*> *vv)
432  {
433  Nring<MeshType> rw = Nring<MeshType> (v, &mesh);
434  do rw.expand (); while (rw.allV.size() < max+1);
435  if (rw.allV[0] != v)
436  printf ("rw.allV[0] != *v\n");
437  vv->reserve ((size_t)max);
438  for (int i = 1; i < max+1; i++)
439  vv->push_back(rw.allV[i]);
440 
441  rw.clear();
442  }
443 
444 
445  static void expandSphereLocal (MeshType & mesh, VertexType *v, float r, int min, std::vector<VertexType*> *vv)
446  {
447  Nring<MeshType> rw = Nring<MeshType> (v, &mesh);
448 
449  bool isInside = true;
450  while (isInside)
451  {
452  rw.expand();
453  vv->reserve(rw.allV.size());
454 
455  typename std::vector<VertexType*>::iterator b = rw.lastV.begin();
456  typename std::vector<VertexType*>::iterator e = rw.lastV.end();
457  isInside = false;
458  while(b != e)
459  {
460  if (((*b)->P() - v->P()).Norm() < r)
461  {
462  vv->push_back(*b);;
463  isInside = true;
464  }
465  ++b;
466  }
467  }
468  //printf ("%d\n", vv->size());
469  rw.clear();
470 
471  if (vv->size() < min)
472  {
473  vv->clear();
474  expandMaxLocal (mesh, v, min, vv);
475  }
476  }
477 
478 
479  static void getAverageNormal (VertexType *vp, std::vector<VertexType*> &vv, CoordType *ppn)
480  {
481  *ppn = CoordType (0,0,0);
482  for (typename std::vector<VertexType*>::iterator vpi = vv.begin(); vpi != vv.end(); ++vpi)
483  *ppn += (*vpi)->N();
484  *ppn += (*vp).N();
485  *ppn /= vv.size() + 1;
486  ppn->Normalize();
487  }
488 
489 
490  static void applyProjOnPlane (CoordType ppn, std::vector<VertexType*> &vin, std::vector<VertexType*> *vout)
491  {
492  for (typename std::vector<VertexType*>::iterator vpi = vin.begin(); vpi != vin.end(); ++vpi)
493  if ((*vpi)->N() * ppn > 0.0f)
494  vout->push_back (*vpi);
495  }
496 
497  static CoordType projectLocal(VertexType* v, VertexType* vp, CoordType ppn)
498  {
499  return vp->P() - (ppn * ((vp->P() - v->P()) * ppn));
500  }
501 
502 
503  static void computeReferenceFramesLocal (VertexType *v, CoordType ppn, std::vector<CoordType> *ref)
504  {
505  vcg::face::VFIterator<FaceType> vfi (v);
506 
507  int i = (vfi.I() + 1) % 3;
508  VertexTypeP vp = vfi.F()->V(i);
509 
510  CoordType x = (projectLocal (v, vp, ppn) - v->P()).Normalize();
511 
512  assert(fabs(x * ppn) < 0.1);
513 
514  *ref = std::vector<CoordType>(3);
515 
516  (*ref)[0] = x;
517  (*ref)[1] = (ppn ^ (*ref)[0]).Normalize();
518  (*ref)[2] = ppn.Normalize(); //ppn / ppn.Norm();
519  }
520 
521 
522  static void fitQuadricLocal (VertexType *v, std::vector<CoordType> ref, std::vector<VertexType*> &vv, QuadricLocal *q)
523  {
524  bool svdResolution = false;
525  bool zeroDeterminantCheck = false;
526 
527  std::vector<CoordType> points;
528  points.reserve (vv.size());
529 
530  typename std::vector<VertexType*>::iterator b = vv.begin();
531  typename std::vector<VertexType*>::iterator e = vv.end();
532 
533  while(b != e)
534  {
535  CoordType cp = (*b)->P();
536 
537  // vtang non e` il v tangente!!!
538  CoordType vTang = cp - v->P();
539 
540  double x = vTang * ref[0];
541  double y = vTang * ref[1];
542  double z = vTang * ref[2];
543  points.push_back(CoordType(x,y,z));
544  ++b;
545  }
546 
547  *q = QuadricLocal::fit (points, svdResolution, zeroDeterminantCheck);
548  }
549 
550 
551  static void finalEigenStuff (VertexType *v, std::vector<CoordType> ref, QuadricLocal q)
552  {
553  double a = q.a();
554  double b = q.b();
555  double c = q.c();
556  double d = q.d();
557  double e = q.e();
558 
559  double E = 1.0 + d*d;
560  double F = d*e;
561  double G = 1.0 + e*e;
562 
563  CoordType n = CoordType(-d,-e,1.0).Normalize();
564 
565  v->N() = ref[0] * n[0] + ref[1] * n[1] + ref[2] * n[2];
566 
567  double L = 2.0 * a * n.Z();
568  double M = b * n.Z();
569  double N = 2 * c * n.Z();
570 
571  // ----------------- Eigen stuff
572  Eigen::Matrix2d m;
573  m << L*G - M*F, M*E-L*F, M*E-L*F, N*E-M*F;
574  m = m / (E*G-F*F);
575  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig(m);
576 
577  Eigen::Vector2d c_val = eig.eigenvalues();
578  Eigen::Matrix2d c_vec = eig.eigenvectors();
579 
580  c_val = -c_val;
581 
582  CoordType v1, v2;
583  v1[0] = c_vec[0];
584  v1[1] = c_vec[1];
585  v1[2] = d * v1[0] + e * v1[1];
586 
587  v2[0] = c_vec[2];
588  v2[1] = c_vec[3];
589  v2[2] = d * v2[0] + e * v2[1];
590 
591  v1 = v1.Normalize();
592  v2 = v2.Normalize();
593 
594  CoordType v1global = ref[0] * v1[0] + ref[1] * v1[1] + ref[2] * v1[2];
595  CoordType v2global = ref[0] * v2[0] + ref[1] * v2[1] + ref[2] * v2[2];
596 
597  v1global.Normalize();
598  v2global.Normalize();
599 
600  v1global *= c_val[0];
601  v2global *= c_val[1];
602 
603  if (c_val[0] > c_val[1])
604  {
605  (*v).PD1() = v1global;
606  (*v).PD2() = v2global;
607  (*v).K1() = c_val[0];
608  (*v).K2() = c_val[1];
609  }
610  else
611  {
612  (*v).PD1() = v2global;
613  (*v).PD2() = v1global;
614  (*v).K1() = c_val[1];
615  (*v).K2() = c_val[0];
616  }
617  // ---- end Eigen stuff
618  }
619 
620 
621 
622  static void updateCurvatureLocal (MeshType & mesh, float radiusSphere)
623  {
624  bool verbose = false;
625  bool projectionPlaneCheck = true;
626  int vertexesPerFit = 0;
627 
628  int i = 0;
629  VertexIterator vi;
630  for(vi = mesh.vert.begin(); vi != mesh.vert.end(); ++vi, i++)
631  {
632  std::vector<VertexType*> vv;
633  std::vector<VertexType*> vvtmp;
634 
635  int count;
636  if (verbose && !((count = (vi - mesh.vert.begin())) % 1000))
637  printf ("vertex %d of %d\n",count,mesh.vert.size());
638 
639  // if (kRing != 0)
640  // expandRing (&*vi, kRing, 5, &vv);
641  // else
642  expandSphereLocal (mesh, &*vi, radiusSphere, 5, &vv);
643 
644  assert (vv.size() >= 5);
645 
646  CoordType ppn;
647  // if (averageNormalMode)
648  // //ppn = (*vi).N();
649  getAverageNormal (&*vi, vv, &ppn);
650  // else
651  // getProjPlaneNormal (&*vi, vv, &ppn);
652 
653  if (projectionPlaneCheck)
654  {
655  vvtmp.reserve (vv.size ());
656  applyProjOnPlane (ppn, vv, &vvtmp);
657  if (vvtmp.size() >= 5)
658  vv = vvtmp;
659  }
660 
661  vvtmp.clear();
662 
663  // if (montecarloMaxVertexNum)
664  // {
665  // //printf ("P: %d\n", vv.size());
666  // vvtmp.reserve (vv.size ());
667  // //printf ("TP: %d\n", vvtmp.size());
668  // applyMontecarlo (montecarloMaxVertexNum, vv, &vvtmp);
669  // //printf ("TD: %d\n", vvtmp.size());
670  // vv = vvtmp;
671  // //printf ("D: %d\n", vv.size());
672  // //printf ("\n");
673  // }
674 
675  assert (vv.size() >= 5);
676 
677  std::vector<CoordType> ref;
678  computeReferenceFramesLocal (&*vi, ppn, &ref);
679 
680  /*
681  printf ("%lf %lf %lf - %lf %lf %lf - %lf %lf %lf\n",
682  ref[0][0], ref[0][1], ref[0][2],
683  ref[1][0], ref[1][1], ref[1][2],
684  ref[2][0], ref[2][1], ref[2][2]);
685  */
686 
687  vertexesPerFit += vv.size();
688  //printf ("size: %d\n", vv.size());
689 
690  QuadricLocal q;
691  fitQuadricLocal (&*vi, ref, vv, &q);
692 
693  finalEigenStuff (&*vi, ref, q);
694 
695  }
696 
697  //if (verbose)
698  //printf ("average vertex num in each fit: %f, total %d, vn %d\n", ((float) vertexesPerFit) / mesh.vn, vertexesPerFit, mesh.vn);
699  if (verbose)
700  printf ("average vertex num in each fit: %f\n", ((float) vertexesPerFit) / mesh.vn);
701  }
702 
703 };
704 
705 }
706 }
707 #endif