DGtal  0.9.3beta
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)
37 
38 #define SphereFittingEstimator_RECURSES
39 
40 #if !defined SphereFittingEstimator_h
41 
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:
112  VectorType m_pos, m_normal;
113  };
114 
115 
116  typedef TSurfel Surfel;
117  typedef TEmbedder SCellEmbedder;
118  typedef typename SCellEmbedder::RealPoint RealPoint;
119 
120  typedef TNormalVectorEstimatorCache NormalVectorEstimatorCache;
121 
122  typedef typename PatatePoint::Scalar Scalar;
124 
125  typedef Grenaille::DistWeightFunc<PatatePoint,Grenaille::SmoothWeightKernel<Scalar> > WeightFunc;
126  typedef Grenaille::Basket<PatatePoint,WeightFunc,Grenaille::OrientedSphereFit, Grenaille::GLSParam> Fit;
127 
129  struct Quantity
130  {
131  RealPoint center;
132  double radius;
133  double tau;
134  double kappa;
135  RealPoint eta;
136 
138  Quantity(RealPoint p, double rad, double _tau,
139  double _kappa, RealPoint _eta): center(p), radius(rad),
140  tau(_tau), kappa(_kappa),
141  eta(_eta) {}
143  bool operator==(Quantity aq) {return (center==aq.center) && (radius==aq.radius);}
144  bool operator<(Quantity aq) {return (center<aq.center) && (radius<aq.radius);}
145  bool operator!=(Quantity aq) {return !(*this == aq);}
146  };
147 
148 
159  const double h,
160  const double radius,
162  myEmbedder(&anEmbedder), myH(h), myNormalEsitmatorCache(&anEstimator)
163  {
164  //From Mellado's example
165  myFit = new Fit();
166  myWeightFunction = new WeightFunc(radius);
167  myFit->setWeightFunc(*myWeightFunction);
168  }
169 
170 
175  {
176  delete myWeightFunction;
177  delete myFit ;
178  }
179 
180 
187  void pushSurfel(const Surfel & aSurf,
188  const double aDistance)
189  {
190  BOOST_VERIFY(aDistance==aDistance);
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
209  myFit->addNeighbor(point);
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));
252  res.radius = myFit->radius();
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 
278  const SCellEmbedder * myEmbedder;
279 
281  Fit *myFit;
282 
284  double myH;
285 
288 
290  const NormalVectorEstimatorCache *myNormalEsitmatorCache;
291 
293  const WeightFunc *myWeightFunction;
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)
SphereFittingEstimator(ConstAlias< SCellEmbedder > anEmbedder, const double h, const double radius, ConstAlias< NormalVectorEstimatorCache > anEstimator)
Trace trace
Definition: Common.h:137
Aim: This class encapsulates its parameter class so that to indicate to the user that the object/poin...
Definition: ConstAlias.h:186
Grenaille::Basket< PatatePoint, WeightFunc, Grenaille::OrientedSphereFit, Grenaille::GLSParam > Fit
Quantity type: a 3-sphere (model of CQuantity)
MULTIARCH PatatePoint(const VectorType &_pos=VectorType::Zero(), const VectorType &_normal=VectorType::Zero())
bool myFirstPoint
Boolean for initial point.
TNormalVectorEstimatorCache NormalVectorEstimatorCache
Grenaille::DistWeightFunc< PatatePoint, Grenaille::SmoothWeightKernel< Scalar > > WeightFunc
DGtal is the top-level namespace which contains all DGtal functions and types.
const SCellEmbedder * myEmbedder
Alias of the geometrical embedder.
void pushSurfel(const Surfel &aSurf, const double aDistance)
std::ostream & info()
const WeightFunc * myWeightFunction
const WeightFunction
Quantity(RealPoint p, double rad, double _tau, double _kappa, RealPoint _eta)
const NormalVectorEstimatorCache * myNormalEsitmatorCache
NormalVectorCache.
Aim: Use Patate library to perform a local sphere fitting.