DGtal  1.1.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)
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:
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;
126  typedef Grenaille::Basket<PatatePoint,WeightFunc,Grenaille::OrientedSphereFit, Grenaille::GLSParam> Fit;
127 
129  struct Quantity
130  {
132  double radius;
133  double tau;
134  double kappa;
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 
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)
DGtal::functors::SphereFittingEstimator::Quantity::operator<
bool operator<(Quantity aq)
Definition: SphereFittingEstimator.h:144
DGtal::functors::SphereFittingEstimator::PatatePoint::pos
MULTIARCH const VectorType & pos() const
Definition: SphereFittingEstimator.h:105
DGtal::functors::SphereFittingEstimator::Quantity::center
RealPoint center
Definition: SphereFittingEstimator.h:131
DGtal::functors::SphereFittingEstimator::myEmbedder
const SCellEmbedder * myEmbedder
Alias of the geometrical embedder.
Definition: SphereFittingEstimator.h:278
DGtal::functors::SphereFittingEstimator::Quantity::~Quantity
~Quantity()
Definition: SphereFittingEstimator.h:142
DGtal::ConstAlias
Aim: This class encapsulates its parameter class so that to indicate to the user that the object/poin...
Definition: ConstAlias.h:187
DGtal::functors::SphereFittingEstimator::Quantity::tau
double tau
Definition: SphereFittingEstimator.h:133
DGtal::functors::SphereFittingEstimator::Quantity::kappa
double kappa
Definition: SphereFittingEstimator.h:134
DGtal::functors::SphereFittingEstimator::myNormalEsitmatorCache
const NormalVectorEstimatorCache * myNormalEsitmatorCache
NormalVectorCache.
Definition: SphereFittingEstimator.h:290
DGtal::functors::SphereFittingEstimator::PatatePoint
Definition: SphereFittingEstimator.h:94
DGtal::functors::SphereFittingEstimator::Quantity::Quantity
Quantity(RealPoint p, double rad, double _tau, double _kappa, RealPoint _eta)
Definition: SphereFittingEstimator.h:138
DGtal::functors::SphereFittingEstimator::myFit
Fit * myFit
Fitting object.
Definition: SphereFittingEstimator.h:281
DGtal::trace
Trace trace
Definition: Common.h:150
DGtal::functors::SphereFittingEstimator::Surfel
TSurfel Surfel
Definition: SphereFittingEstimator.h:116
DGtal::functors::SphereFittingEstimator::VectorType
PatatePoint::VectorType VectorType
Definition: SphereFittingEstimator.h:123
DGtal::functors::SphereFittingEstimator::WeightFunc
Grenaille::DistWeightFunc< PatatePoint, Grenaille::SmoothWeightKernel< Scalar > > WeightFunc
Definition: SphereFittingEstimator.h:125
DGtal::functors::SphereFittingEstimator::PatatePoint::normal
MULTIARCH VectorType & normal()
Definition: SphereFittingEstimator.h:109
DGtal::functors::SphereFittingEstimator
Aim: Use Patate library to perform a local sphere fitting.
Definition: SphereFittingEstimator.h:89
DGtal::functors::SphereFittingEstimator::Quantity::operator!=
bool operator!=(Quantity aq)
Definition: SphereFittingEstimator.h:145
DGtal::functors::SphereFittingEstimator::PatatePoint::VectorType
Eigen::Matrix< Scalar, Dim, 1 > VectorType
Definition: SphereFittingEstimator.h:98
DGtal::functors::SphereFittingEstimator::Quantity::operator==
bool operator==(Quantity aq)
Definition: SphereFittingEstimator.h:143
DGtal::functors::SphereFittingEstimator::pushSurfel
void pushSurfel(const Surfel &aSurf, const double aDistance)
Definition: SphereFittingEstimator.h:187
DGtal::functors::SphereFittingEstimator::NormalVectorEstimatorCache
TNormalVectorEstimatorCache NormalVectorEstimatorCache
Definition: SphereFittingEstimator.h:120
DGtal::Trace::info
std::ostream & info()
DGtal::functors::SphereFittingEstimator::myWeightFunction
const WeightFunc * myWeightFunction
const WeightFunction
Definition: SphereFittingEstimator.h:293
DGtal::functors::SphereFittingEstimator::Quantity
Quantity type: a 3-sphere (model of CQuantity)
Definition: SphereFittingEstimator.h:130
DGtal::functors::SphereFittingEstimator::PatatePoint::m_normal
VectorType m_normal
Definition: SphereFittingEstimator.h:112
DGtal::functors::SphereFittingEstimator::~SphereFittingEstimator
~SphereFittingEstimator()
Definition: SphereFittingEstimator.h:174
DGtal::functors::SphereFittingEstimator::SphereFittingEstimator
SphereFittingEstimator(ConstAlias< SCellEmbedder > anEmbedder, const double h, const double radius, ConstAlias< NormalVectorEstimatorCache > anEstimator)
Definition: SphereFittingEstimator.h:158
DGtal
DGtal is the top-level namespace which contains all DGtal functions and types.
Definition: ClosedIntegerHalfPlane.h:49
DGtal::functors::SphereFittingEstimator::SCellEmbedder
TEmbedder SCellEmbedder
Definition: SphereFittingEstimator.h:117
DGtal::functors::SphereFittingEstimator::myH
double myH
Grid step.
Definition: SphereFittingEstimator.h:284
DGtal::functors::SphereFittingEstimator::PatatePoint::Dim
@ Dim
Definition: SphereFittingEstimator.h:96
DGtal::functors::SphereFittingEstimator::Scalar
PatatePoint::Scalar Scalar
Definition: SphereFittingEstimator.h:122
DGtal::functors::SphereFittingEstimator::reset
void reset()
Definition: SphereFittingEstimator.h:266
DGtal::functors::SphereFittingEstimator::PatatePoint::pos
MULTIARCH VectorType & pos()
Definition: SphereFittingEstimator.h:108
DGtal::functors::SphereFittingEstimator::PatatePoint::MatrixType
Eigen::Matrix< Scalar, Dim, Dim > MatrixType
Definition: SphereFittingEstimator.h:99
DGtal::functors::SphereFittingEstimator::PatatePoint::Scalar
double Scalar
Definition: SphereFittingEstimator.h:97
DGtal::functors::SphereFittingEstimator::Quantity::eta
RealPoint eta
Definition: SphereFittingEstimator.h:135
DGtal::functors::SphereFittingEstimator::Fit
Grenaille::Basket< PatatePoint, WeightFunc, Grenaille::OrientedSphereFit, Grenaille::GLSParam > Fit
Definition: SphereFittingEstimator.h:126
DGtal::functors::SphereFittingEstimator::eval
Quantity eval()
Definition: SphereFittingEstimator.h:221
DGtal::functors::SphereFittingEstimator::Quantity::Quantity
Quantity()
Definition: SphereFittingEstimator.h:137
DGtal::functors::SphereFittingEstimator::PatatePoint::m_pos
VectorType m_pos
Definition: SphereFittingEstimator.h:112
DGtal::functors::SphereFittingEstimator::myFirstPoint
bool myFirstPoint
Boolean for initial point.
Definition: SphereFittingEstimator.h:287
DGtal::functors::SphereFittingEstimator::PatatePoint::PatatePoint
MULTIARCH PatatePoint(const VectorType &_pos=VectorType::Zero(), const VectorType &_normal=VectorType::Zero())
Definition: SphereFittingEstimator.h:101
DGtal::functors::SphereFittingEstimator::PatatePoint::normal
MULTIARCH const VectorType & normal() const
Definition: SphereFittingEstimator.h:106
RealPoint
Z2i::RealPoint RealPoint
Definition: testAstroid2D.cpp:46
DGtal::functors::SphereFittingEstimator::Quantity::radius
double radius
Definition: SphereFittingEstimator.h:132
DGtal::functors::SphereFittingEstimator::RealPoint
SCellEmbedder::RealPoint RealPoint
Definition: SphereFittingEstimator.h:118