DGtal  1.2.0
SphereFittingEstimator.h
1
17 #pragma once
18
34 #if defined(SphereFittingEstimator_RECURSES)
35 #error Recursive header files inclusion detected in SphereFittingEstimator.h
36 #else // defined(SphereFittingEstimator_RECURSES)
38 #define SphereFittingEstimator_RECURSES
39
40 #if !defined SphereFittingEstimator_h
42 #define SphereFittingEstimator_h
43
45 // Inclusions
46 #include <iostream>
47 #include <DGtal/base/Common.h>
48 #include <DGtal/topology/SCellsFunctors.h>
49
50 #ifndef WITH_PATATE
51 #error You need to have activated Patate (WITH_PATATE) to include this file.
52 #endif
53
54 //Patate includes
55 #include <Patate/grenaille.h>
56 #include <Eigen/Eigen>
57 #include <vector>
58
60
61 namespace DGtal
62 {
63  namespace functors
64  {
66  // template class SphereFittingEstimator
85  template <typename TSurfel,
86  typename TEmbedder,
87  typename TNormalVectorEstimatorCache>
89  {
90  public:
91
92
94  {
95  public:
96  enum {Dim = 3};
97  typedef double Scalar;
98  typedef Eigen::Matrix<Scalar, Dim, 1> VectorType;
99  typedef Eigen::Matrix<Scalar, Dim, Dim> MatrixType;
100
101  MULTIARCH inline PatatePoint(const VectorType& _pos = VectorType::Zero(),
102  const VectorType& _normal = VectorType::Zero())
103  : m_pos(_pos), m_normal(_normal) {}
104
105  MULTIARCH inline const VectorType& pos() const { return m_pos; }
106  MULTIARCH inline const VectorType& normal() const { return m_normal; }
107
108  MULTIARCH inline VectorType& pos() { return m_pos; }
109  MULTIARCH inline VectorType& normal() { return m_normal; }
110
111  private:
113  };
114
115
116  typedef TSurfel Surfel;
117  typedef TEmbedder SCellEmbedder;
119
120  typedef TNormalVectorEstimatorCache NormalVectorEstimatorCache;
121
122  typedef typename PatatePoint::Scalar Scalar;
124
125  typedef Grenaille::DistWeightFunc<PatatePoint,Grenaille::SmoothWeightKernel<Scalar> > WeightFunc;
127
129  struct Quantity
130  {
133  double tau;
134  double kappa;
136
138  Quantity(RealPoint p, double rad, double _tau,
140  tau(_tau), kappa(_kappa),
141  eta(_eta) {}
145  bool operator!=(Quantity aq) {return !(*this == aq);}
146  };
147
148
159  const double h,
162  myEmbedder(&anEmbedder), myH(h), myNormalEsitmatorCache(&anEstimator)
163  {
165  myFit = new Fit();
167  myFit->setWeightFunc(*myWeightFunction);
168  }
169
170
175  {
176  delete myWeightFunction;
177  delete myFit ;
178  }
179
180
187  void pushSurfel(const Surfel & aSurf,
189  {
191
192  RealPoint p = myEmbedder->operator()(aSurf);
193  RealPoint norm = myNormalEsitmatorCache->eval(aSurf);
194  VectorType pp;
195  pp(0) = p[0]*myH;
196  pp(1) = p[1]*myH;
197  pp(2) = p[2]*myH;
198  VectorType normal;
199  normal(0) = norm[0];
200  normal(1) = norm[1];
201  normal(2) = norm[2];
202  PatatePoint point(pp, normal);
203  if (myFirstPoint)
204  {
205  myFirstPoint = false;
206  myFit->init(pp);
207  }
208  else
210
211 #ifdef DEBUG_VERBOSE
212  trace.info() <<"#";
213 #endif
214  }
215
222  {
223  myFit->finalize();
224
225 #ifdef DEBUG_VERBOSE
226  trace.info() <<std::endl;
227
228  //Test if the fitting ended without errors
229  if(myFit->isStable())
230  {
231  std::cout << "Center: [" << myFit->center().transpose() << "] ; radius: " << myFit->radius() << std::endl;
232
233  std::cout << "Pratt normalization"
234  << (myFit->applyPrattNorm() ? " is now done." : " has already been applied.") << std::endl;
235
236
237  std::cout << "Fitted Sphere: " << std::endl
238  << "\t Tau : " << myFit->tau() << std::endl
239  << "\t Eta : " << myFit->eta().transpose() << std::endl
240  << "\t Kappa: " << myFit->kappa() << std::endl;
241
242  }
243  else
244  {
245  std::cout << "Ooops... not stable result"<<std::endl;
246  }
247 #endif
248  Quantity res;
249  res.center = RealPoint((myFit->center())(0),
250  (myFit->center())(1),
251  (myFit->center())(2));
253  res.tau = myFit->tau();
254  res.kappa = myFit->kappa();
255  res.eta = RealPoint((myFit->eta())(0),
256  (myFit->eta())(1),
257  (myFit->eta())(2));
258  return res;
259  }
260
261
266  void reset()
267  {
268  delete myFit;
269  myFit = new Fit();
270  myFirstPoint = true;
271  myFit->setWeightFunc(*myWeightFunction);
272  }
273
274
275  private:
276
279
282
284  double myH;
285
288
291
294
295  }; // end of class SphereFittingEstimator
296  }
297 } // namespace DGtal
298
299
300 // //
302
303 #endif // !defined SphereFittingEstimator_h
304
305 #undef SphereFittingEstimator_RECURSES
306 #endif // else defined(SphereFittingEstimator_RECURSES)
Aim: This class encapsulates its parameter class so that to indicate to the user that the object/poin...
Definition: ConstAlias.h:187
std::ostream & info()
MULTIARCH PatatePoint(const VectorType &_pos=VectorType::Zero(), const VectorType &_normal=VectorType::Zero())
Aim: Use Patate library to perform a local sphere fitting.
const NormalVectorEstimatorCache * myNormalEsitmatorCache
NormalVectorCache.
bool myFirstPoint
Boolean for initial point.
Grenaille::DistWeightFunc< PatatePoint, Grenaille::SmoothWeightKernel< Scalar > > WeightFunc
const SCellEmbedder * myEmbedder
Alias of the geometrical embedder.
const WeightFunc * myWeightFunction
const WeightFunction
SphereFittingEstimator(ConstAlias< SCellEmbedder > anEmbedder, const double h, const double radius, ConstAlias< NormalVectorEstimatorCache > anEstimator)
Grenaille::Basket< PatatePoint, WeightFunc, Grenaille::OrientedSphereFit, Grenaille::GLSParam > Fit
TNormalVectorEstimatorCache NormalVectorEstimatorCache
void pushSurfel(const Surfel &aSurf, const double aDistance)
DGtal is the top-level namespace which contains all DGtal functions and types.
Trace trace
Definition: Common.h:154
Quantity type: a 3-sphere (model of CQuantity)
Quantity(RealPoint p, double rad, double _tau, double _kappa, RealPoint _eta)
Z2i::RealPoint RealPoint