Debian Codegraph @

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

raw
1 #include "PlanePrimitiveShape.h"
2 #include "ScoreComputer.h"
3 #include "PrimitiveShapeVisitor.h"
4 #include <GfxTL/NullClass.h>
5 #include <limits>
6 #include <iostream>
7 #include <fstream>
8 #include <sstream>
9 #include <iomanip>
10 #include <MiscLib/Performance.h>
11 extern MiscLib::performance_t totalTime_planeConnected;
12
13 PlanePrimitiveShape::PlanePrimitiveShape(const Vec3f &a, const Vec3f &b,
14 const Vec3f &c)
15 : m_plane(a, b, c)
16 {
17 m_hcs.FromNormal(m_plane.getNormal());
18 }
19
20 PlanePrimitiveShape::PlanePrimitiveShape(const Plane &plane)
21 : m_plane(plane)
22 {
23 m_hcs.FromNormal(m_plane.getNormal());
24 }
25
26 size_t PlanePrimitiveShape::Identifier() const
27 {
28 return 0;
29 }
30
31 PrimitiveShape *PlanePrimitiveShape::Clone() const
32 {
33 return new PlanePrimitiveShape(*this);
34 }
35
36 float PlanePrimitiveShape::Distance(const Vec3f &p) const
37 {
38 return m_plane.getDistance(p);
39 }
40
41 float PlanePrimitiveShape::SignedDistance(const Vec3f &p) const
42 {
43 return m_plane.SignedDistance(p);
44 }
45
46 float PlanePrimitiveShape::NormalDeviation(const Vec3f &,
47 const Vec3f &n) const
48 {
49 return m_plane.getNormal().dot(n);
50 }
51
52 void PlanePrimitiveShape::DistanceAndNormalDeviation(
53 const Vec3f &p, const Vec3f &n, std::pair< float, float > *dn) const
54 {
55 dn->first = m_plane.getDistance(p);
56 dn->second = m_plane.getNormal().dot(n);
57 }
58
59 void PlanePrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const
60 {
61 *pp = p - ((m_plane.getNormal().dot(p - m_plane.getPosition()))
62 * m_plane.getNormal());
63 }
64
65 void PlanePrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const
66 {
67 *n = m_plane.getNormal();
68 }
69
70 unsigned int PlanePrimitiveShape::ConfidenceTests(unsigned int numTests,
71 float epsilon, float normalThresh, float rms, const PointCloud &pc,
72 const MiscLib::Vector< size_t > &indices) const
73 {
74 return BasePrimitiveShape::ConfidenceTests< Plane >(numTests, epsilon,
75 normalThresh, rms, pc, indices);
76 }
77
78 void PlanePrimitiveShape::Description(std::string *s) const
79 {
80 *s = "Plane";
81 }
82
83 bool PlanePrimitiveShape::Fit(const PointCloud &pc, float epsilon,
84 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
85 MiscLib::Vector< size_t >::const_iterator end)
86 {
87 Plane fit = m_plane;
88 //if(fit.LeastSquaresFit(pc, begin, end))
89 if(fit.LeastSquaresFit(pc, begin, end))
90 {
91 m_plane = fit;
92 m_hcs.FromNormal(m_plane.getNormal());
93 return true;
94 }
95 return false;
96 }
97
98 PrimitiveShape *PlanePrimitiveShape::LSFit(const PointCloud &pc, float epsilon,
99 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
100 MiscLib::Vector< size_t >::const_iterator end,
101 std::pair< size_t, float > *score) const
102 {
103 Plane fit = m_plane;
104 if(fit.LeastSquaresFit(pc, begin, end))
105 {
106 score->first = -1;
107 return new PlanePrimitiveShape(fit);
108 }
109 score->first = 0;
110 return NULL;
111 }
112
113 void PlanePrimitiveShape::Serialize(std::ostream *o, bool binary) const
114 {
115 if(binary)
116 {
117 const char id = 0;
118 (*o) << id;
119 }
120 else
121 (*o) << "0" << " ";
122 m_plane.Serialize(binary, o);
123 if(!binary)
124 *o << std::endl;
125 }
126
127 size_t PlanePrimitiveShape::SerializedSize() const
128 {
129 return m_plane.SerializedSize() + 1;
130 }
131
132 void PlanePrimitiveShape::Transform(float scale, const Vec3f &translate)
133 {
134 m_plane.Transform(scale, translate);
135 }
136
137 void PlanePrimitiveShape::Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
138 const GfxTL::Vector3Df &trans)
139 {
140 GfxTL::Vector3Df transNormal = rot * GfxTL::Vector3Df(
141 m_plane.getNormal());
142 GfxTL::Vector3Df transOrigin = rot * GfxTL::Vector3Df(
143 m_plane.getPosition()) + trans;
144 m_plane = Plane(Vec3f(transOrigin.Data()), Vec3f(transNormal.Data()));
145 m_hcs[0] = rot * m_hcs[0];
146 m_hcs[1] = rot * m_hcs[1];
147 }
148
149 void PlanePrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const
150 {
151 visitor->Visit(*this);
152 }
153
154 bool PlanePrimitiveShape::Similar(float, const PlanePrimitiveShape &) const
155 {
156 return true; // planes are always similar!
157 }
158
159 LevMarFunc< float > *PlanePrimitiveShape::SignedDistanceFunc() const
160 {
161 return new PlaneLevMarFunc(m_plane);
162 }
163
164 void PlanePrimitiveShape::Parameters(const Vec3f &p,
165 std::pair< float, float > *param) const
166 {
167 Vec3f pp = p - m_plane.getPosition();
168 param->first = pp.dot(m_hcs[0].Data());
169 param->second = pp.dot(m_hcs[1].Data());
170 }
171
172 void PlanePrimitiveShape::Parameters(
173 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
174 PointCloud::const_iterator > begin,
175 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
176 PointCloud::const_iterator > end,
177 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
178 {
179 ParametersImpl(begin, end, bmpParams);
180 }
181
182 void PlanePrimitiveShape::Parameters(
183 GfxTL::IndexedIterator< IndexIterator,
184 PointCloud::const_iterator > begin,
185 GfxTL::IndexedIterator< IndexIterator,
186 PointCloud::const_iterator > end,
187 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
188 {
189 ParametersImpl(begin, end, bmpParams);
190 }
191
192 void PlanePrimitiveShape::BitmapExtent(float epsilon,
193 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
194 MiscLib::Vector< std::pair< float, float > > *params,
195 size_t *uextent, size_t *vextent)
196 {
197 *uextent = size_t(std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon)) + 1;
198 *vextent = size_t(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon)) + 1;
199 }
200
201 void PlanePrimitiveShape::InBitmap(const std::pair< float, float > &param,
202 float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t,
203 size_t, std::pair< int, int > *inBmp) const
204 {
205 inBmp->first = (int)std::floor((param.first - bbox.Min()[0]) / epsilon);
206 inBmp->second = (int)std::floor((param.second - bbox.Min()[1]) / epsilon);
207 }
208 void PlanePrimitiveShape::WrapBitmap(
209 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, bool *uwrap,
210 bool *vwrap) const
211 {
212 *uwrap = false;
213 *vwrap = false;
214 }
215
216 void PlanePrimitiveShape::SetExtent(
217 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, const MiscLib::Vector< int > &,
218 size_t, size_t, float, int)
219 {}
220
221 bool PlanePrimitiveShape::InSpace(float u, float v, Vec3f *p, Vec3f *n) const
222 {
223 *p = m_plane.getPosition() + Vec3f((u * m_hcs[0] + v * m_hcs[1]).Data());
224 *n = m_plane.getNormal();
225 return true;
226 }
227
228 bool PlanePrimitiveShape::InSpace(size_t u, size_t v, float epsilon,
229 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
230 size_t vextent, Vec3f *p, Vec3f *n) const
231 {
232 *p = Vec3f(((bbox.Min()[0] + epsilon * (float(u) + .5f)) * m_hcs[0] +
233 (bbox.Min()[1] + epsilon * (float(v) + .5f)) * m_hcs[1]).Data()) +
234 m_plane.getPosition();
235 *n = m_plane.getNormal();
236 return true;
237 }