DGtal 1.3.0
Loading...
Searching...
No Matches
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
61namespace 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;
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 {
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)
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)