Debian Codegraph @

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

raw
1 #ifndef PLANEPRIMITIVESHAPE_HEADER
2 #define PLANEPRIMITIVESHAPE_HEADER
3 #include "BitmapPrimitiveShape.h"
4 #include "Plane.h"
5 #include <GfxTL/HyperplaneCoordinateSystem.h>
6 #include <GfxTL/AABox.h>
7 #include <GfxTL/MatrixXX.h>
8 #include <GfxTL/VectorXD.h>
9 #include "LevMarFunc.h"
10
11 #ifndef DLL_LINKAGE
12 #define DLL_LINKAGE
13 #endif
14
15 class DLL_LINKAGE PlanePrimitiveShape
16 : public BitmapPrimitiveShape
17 {
18 public:
19 PlanePrimitiveShape(const Vec3f &a, const Vec3f &b, const Vec3f &c);
20 PlanePrimitiveShape(const Plane &plane);
21 size_t Identifier() const;
22 unsigned int RequiredSamples() const { return Plane::RequiredSamples; }
23 PrimitiveShape *Clone() const;
24 float Distance(const Vec3f &p) const;
25 float SignedDistance(const Vec3f &p) const;
26 float NormalDeviation(const Vec3f &p, const Vec3f &n) const;
27 void DistanceAndNormalDeviation(const Vec3f &p, const Vec3f &n,
28 std::pair< float, float > *dn) const;
29 void Project(const Vec3f &p, Vec3f *pp) const;
30 void Normal(const Vec3f &p, Vec3f *n) const;
31 unsigned int ConfidenceTests(unsigned int numTests, float epsilon,
32 float normalThresh, float rms, const PointCloud &pc,
33 const MiscLib::Vector< size_t > &indices) const;
34 void Description(std::string *s) const;
35 bool Fit(const PointCloud &pc, float epsilon, float normalThresh,
36 MiscLib::Vector< size_t >::const_iterator begin,
37 MiscLib::Vector< size_t >::const_iterator end);
38 PrimitiveShape *LSFit(const PointCloud &pc, float epsilon,
39 float normalThresh,
40 MiscLib::Vector< size_t >::const_iterator begin,
41 MiscLib::Vector< size_t >::const_iterator end,
42 std::pair< size_t, float > *score) const;
43 LevMarFunc< float > *SignedDistanceFunc() const;
44 void Serialize(std::ostream *o, bool binary = true) const;
45 size_t SerializedSize() const;
46 virtual void Serialize(float* array) const {m_plane.Serialize(array);}
47 virtual size_t SerializedFloatSize() const {return m_plane.SerializedFloatSize();}
48 void Transform(float scale, const Vec3f &translate);
49 void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
50 const GfxTL::Vector3Df &trans);
51 void Visit(PrimitiveShapeVisitor *visitor) const;
52 bool Similar(float tolerance, const PlanePrimitiveShape &) const;
53 const Plane &Internal() const { return m_plane; }
54
55 void Parameters(const Vec3f &p,
56 std::pair< float, float > *param) const;
57 void Parameters(GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
58 PointCloud::const_iterator > begin,
59 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
60 PointCloud::const_iterator > end,
61 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
62 void Parameters(GfxTL::IndexedIterator< IndexIterator,
63 PointCloud::const_iterator > begin,
64 GfxTL::IndexedIterator< IndexIterator,
65 PointCloud::const_iterator > end,
66 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
67 bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const;
68 void BitmapExtent(float epsilon,
69 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
70 MiscLib::Vector< std::pair< float, float > > *params,
71 size_t *uextent, size_t *vextent);
72 void InBitmap(const std::pair< float, float > &param, float epsilon,
73 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
74 size_t vextent, std::pair< int, int > *inBmp) const;
75 void WrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
76 float epsilon, bool *uwrap, bool *vwrap) const;
77 void SetExtent(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
78 const MiscLib::Vector< int > &componentsImg, size_t uextent,
79 size_t vextent, float epsilon, int label);
80 bool InSpace(size_t u, size_t v, float epsilon,
81 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
82 size_t vextent, Vec3f *p, Vec3f *n) const;
83
84 Vec3f getXDim() const {return Vec3f(m_hcs[0].Data());};
85 Vec3f getYDim() const {return Vec3f(m_hcs[1].Data());};
86
87 private:
88 template< class IteratorT >
89 void ParametersImpl(IteratorT begin, IteratorT end,
90 MiscLib::Vector< std::pair< float, float > > *bmpParams) const;
91
92 private:
93 Plane m_plane;
94 GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
95 };
96
97 template< class IteratorT >
98 void PlanePrimitiveShape::ParametersImpl(IteratorT begin, IteratorT end,
99 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
100 {
101 bmpParams->resize(end - begin);
102 size_t j = 0;
103 for(IteratorT i = begin; i != end; ++i, ++j)
104 {
105 Vec3f pp = *i - m_plane.getPosition();
106 (*bmpParams)[j].first = pp.dot(m_hcs[0].Data());
107 (*bmpParams)[j].second = pp.dot(m_hcs[1].Data());
108 }
109 }
110
111 class DLL_LINKAGE PlaneLevMarFunc
112 : public LevMarFunc< float >
113 {
114 public:
115 PlaneLevMarFunc(const Plane &plane)
116 {
117 for(unsigned int i = 0; i < 3; ++i)
118 m_plane[i] = plane.getNormal()[i];
119 m_plane[3] = plane.SignedDistToOrigin();
120 }
121
122 float operator()(const float *x) const
123 {
124 return m_plane[0] * x[0] + m_plane[1] * x[1] +
125 m_plane[2] * x[2] - m_plane[3];
126 }
127
128 void operator()(const float *x, float *gradient) const
129 {
130 for(unsigned int i = 0; i < 3; ++i)
131 gradient[i] = m_plane[i];
132 }
133
134 private:
135 GfxTL::Vector4Df m_plane;
136 };
137
138 #endif