Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/ConePrimitiveShape.h

raw
1 #ifndef CONEPRIMITIVESHAPE_HEADER
2 #define CONEPRIMITIVESHAPE_HEADER
3 #include "BitmapPrimitiveShape.h"
4 #include "Cone.h"
5 #include "LevMarFunc.h"
6
7 #ifndef DLL_LINKAGE
8 #define DLL_LINKAGE
9 #endif
10
11 class DLL_LINKAGE ConePrimitiveShape
12 : public BitmapPrimitiveShape
13 {
14 public:
15 size_t Identifier() const;
16 unsigned int RequiredSamples() const { return Cone::RequiredSamples; }
17 ConePrimitiveShape(const Cone &cone, float maxRadius = std::numeric_limits<float>::infinity(), float maxAngle = std::numeric_limits<float>::infinity(), float maxLength = std::numeric_limits<float>::infinity());
18 PrimitiveShape* Clone() const;
19 float Distance(const Vec3f &p) const;
20 float SignedDistance(const Vec3f &p) const;
21 float NormalDeviation(const Vec3f &p, const Vec3f &n) const;
22 void DistanceAndNormalDeviation(const Vec3f &p,
23 const Vec3f &n, std::pair< float, float > *dn) const;
24 void Project(const Vec3f &p, Vec3f *pp) const;
25 void Normal(const Vec3f &p, Vec3f *n) const;
26 unsigned int ConfidenceTests(unsigned int numTests, float epsilon,
27 float normalThresh, float rms, const PointCloud &pc,
28 const MiscLib::Vector< size_t > &indices) const;
29 void Description(std::string *s) const;
30 bool Fit(const PointCloud &pc, float epsilon, float normalThresh,
31 MiscLib::Vector< size_t >::const_iterator begin,
32 MiscLib::Vector< size_t >::const_iterator end);
33 PrimitiveShape *LSFit(const PointCloud &pc, float epsilon,
34 float normalThresh,
35 MiscLib::Vector< size_t >::const_iterator begin,
36 MiscLib::Vector< size_t >::const_iterator end,
37 std::pair< size_t, float > *score) const;
38 LevMarFunc< float > *SignedDistanceFunc() const;
39 void Serialize(std::ostream *o, bool binary = true) const;
40 size_t SerializedSize() const;
41 virtual void Serialize(float* array) const {m_cone.Serialize(array);}
42 virtual size_t SerializedFloatSize() const {return m_cone.SerializedFloatSize();}
43 void Transform(float scale, const Vec3f &translate);
44 void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
45 const GfxTL::Vector3Df &trans);
46 void Visit(PrimitiveShapeVisitor *visitor) const;
47 void SuggestSimplifications(const PointCloud &pc,
48 MiscLib::Vector< size_t >::const_iterator begin,
49 MiscLib::Vector< size_t >::const_iterator end, float distThresh,
50 MiscLib::Vector< MiscLib::RefCountPtr< PrimitiveShape > > *suggestions) const;
51 bool Similar(float tolerance, const ConePrimitiveShape &shape) const;
52 const Cone &Internal() const { return m_cone; }
53
54 void Parameters(const Vec3f &p,
55 std::pair< float, float > *param) const;
56 void Parameters(GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
57 PointCloud::const_iterator > begin,
58 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
59 PointCloud::const_iterator > end,
60 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
61 void Parameters(GfxTL::IndexedIterator< IndexIterator,
62 PointCloud::const_iterator > begin,
63 GfxTL::IndexedIterator< IndexIterator,
64 PointCloud::const_iterator > end,
65 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
66 bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const;
67 void BitmapExtent(float epsilon,
68 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
69 MiscLib::Vector< std::pair< float, float > > *params,
70 size_t *uextent, size_t *vextent);
71 void InBitmap(const std::pair< float, float > &param, float epsilon,
72 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
73 size_t vextent, std::pair< int, int > *inBmp) const;
74 void PreWrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
75 float epsilon, size_t uextent, size_t vextent,
76 MiscLib::Vector< char > *bmp) const;
77 void WrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
78 float epsilon, bool *uwrap, bool *vwrap) const;
79 void WrapComponents(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
80 float epsilon, size_t uextent, size_t vextent,
81 MiscLib::Vector< int > *componentImg,
82 MiscLib::Vector< std::pair< int, size_t > > *labels) const;
83 void SetExtent(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
84 const MiscLib::Vector< int > &componentsImg, size_t uextent,
85 size_t vextent, float epsilon, int label);
86 void SetExtent(const GfxTL::AABox< GfxTL::Vector2Df > &bbox);
87 bool InSpace(size_t u, size_t v, float epsilon,
88 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
89 size_t vextent, Vec3f *p, Vec3f *n) const;
90 bool CheckGeneratedShapeWithinLimits(const PointCloud& pc,
91 MiscLib::Vector< size_t >::const_iterator begin,
92 MiscLib::Vector< size_t >::const_iterator end) override
93 {
94 if (m_cone.RadiusAtLength(m_extBbox.Min()[0]) <= m_maxRadius &&
95 m_cone.RadiusAtLength(m_extBbox.Max()[0]) <= m_maxRadius &&
96 m_cone.RadiusAtLength(m_extBbox.Min()[1]) <= m_maxRadius &&
97 m_cone.RadiusAtLength(m_extBbox.Max()[1]) <= m_maxRadius &&
98 m_cone.Angle() <= m_maxAngle)
99 {
100 return true;
101 }
102 return false;
103 }
104
105 private:
106 template< class IteratorT >
107 void ParametersImpl(IteratorT begin, IteratorT end,
108 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
109
110 private:
111 Cone m_cone;
112 float m_maxRadius;
113 float m_maxLength;
114 float m_maxAngle;
115 };
116
117 template< class IteratorT >
118 void ConePrimitiveShape::ParametersImpl(IteratorT begin, IteratorT end,
119 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
120 {
121 bmpParams->resize(end - begin);
122 size_t j = 0;
123 for(IteratorT i = begin; i != end; ++i, ++j)
124 m_cone.Parameters(*i, &(*bmpParams)[j]);
125 if(m_cone.Angle() < float(M_PI / 4)) // angle of cone less than 45 degrees
126 {
127 for(size_t j = 0; j < bmpParams->size(); ++j)
128 {
129 float r = m_cone.RadiusAtLength((*bmpParams)[j].first);
130 // convert to arc length and centralize
131 (*bmpParams)[j].second = ((*bmpParams)[j].second - float(M_PI)) * r;
132 }
133 }
134 else
135 {
136 for(size_t j = 0; j < bmpParams->size(); ++j)
137 {
138 float l = (*bmpParams)[j].first;
139 (*bmpParams)[j].first = std::sin((*bmpParams)[j].second) * l;
140 (*bmpParams)[j].second = std::cos((*bmpParams)[j].second) * l;
141 }
142 }
143 }
144
145 class DLL_LINKAGE ConeLevMarFunc
146 : public LevMarFunc< float >
147 {
148 public:
149 ConeLevMarFunc(const Cone &c)
150 : m_cone(c)
151 {}
152
153 float operator()(const float *x) const
154 {
155 return m_cone.SignedDistance(*((const Vec3f *)x));
156 }
157
158 void operator()(const float *x, float *gradient) const
159 {
160 m_cone.Normal(*((const Vec3f *)x), (Vec3f *)gradient);
161 }
162
163 private:
164 Cone m_cone;
165 };
166
167 #endif