Debian Codegraph @

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

raw
1 #ifndef CYLINDER_HEADER
2 #define CYLINDER_HEADER
3 #include "basic.h"
4 #include <stdexcept>
5 #include <utility>
6 #include <MiscLib/Vector.h>
7 #include "PointCloud.h"
8 #include <ostream>
9 #include <istream>
10 #include <GfxTL/HyperplaneCoordinateSystem.h>
11 #include <stdio.h>
12 #include <MiscLib/NoShrinkVector.h>
13 #include "LevMarLSWeight.h"
14 #include "LevMarFitting.h"
15
16 #ifndef DLL_LINKAGE
17 #define DLL_LINKAGE
18 #endif
19
20 class DLL_LINKAGE Cylinder
21 {
22 public:
23 struct ParallelNormalsError
24 : public std::runtime_error
25 {
26 ParallelNormalsError();
27 };
28 enum { RequiredSamples = 2 };
29 Cylinder();
30 Cylinder(const Vec3f &axisDir, const Vec3f &axisPos, float radius);
31 Cylinder(const Vec3f &pointA, const Vec3f &pointB,
32 const Vec3f &normalA, const Vec3f &normalB);
33 bool Init(const MiscLib::Vector< Vec3f > &samples);
34 bool InitAverage(const MiscLib::Vector< Vec3f > &samples);
35 bool Init(const Vec3f &axisDir, const Vec3f &axisPos, float radius);
36 bool Init(const Vec3f &pointA, const Vec3f &pointB,
37 const Vec3f &normalA, const Vec3f &normalB);
38 bool Init(bool binary, std::istream *i);
39 void Init(FILE *i);
40 void Init(float* array);
41 inline float Distance(const Vec3f &p) const;
42 inline void Normal(const Vec3f &p, Vec3f *normal) const;
43 inline float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const;
44 inline float SignedDistance(const Vec3f &p) const;
45 void Project(const Vec3f &p, Vec3f *pp) const;
46 // parameters are (height, angle)
47 void Parameters(const Vec3f &p,
48 std::pair< float, float > *param) const;
49 float Radius() const;
50 float &Radius();
51 const Vec3f &AxisDirection() const;
52 Vec3f &AxisDirection();
53 const Vec3f &AxisPosition() const;
54 Vec3f &AxisPosition();
55 const Vec3f AngularDirection() const;
56 void RotateAngularDirection(float radians);
57 bool LeastSquaresFit(const PointCloud &pc,
58 MiscLib::Vector< size_t >::const_iterator begin,
59 MiscLib::Vector< size_t >::const_iterator end);
60 template< class IteratorT >
61 bool LeastSquaresFit(IteratorT begin, IteratorT end);
62 bool Fit(const PointCloud &pc,
63 MiscLib::Vector< size_t >::const_iterator begin,
64 MiscLib::Vector< size_t >::const_iterator end)
65 { return LeastSquaresFit(pc, begin, end); }
66 static bool Interpolate(const MiscLib::Vector< Cylinder > &cylinders,
67 const MiscLib::Vector< float > &weights, Cylinder *ic);
68 void Serialize(bool binary, std::ostream *o) const;
69 static size_t SerializedSize();
70 void Serialize(FILE *o) const;
71 void Serialize(float* array) const;
72 static size_t SerializedFloatSize();
73
74 void Transform(float scale, const Vec3f &translate);
75 void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
76 const GfxTL::Vector3Df &trans);
77 inline unsigned int Intersect(const Vec3f &p, const Vec3f &r,
78 float *first, float *second) const;
79
80 private:
81 template< class WeightT >
82 class LevMarCylinder
83 : public WeightT
84 {
85 public:
86 enum { NumParams = 7 };
87 typedef float ScalarType;
88
89 template< class IteratorT >
90 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
91 ScalarType *values, ScalarType *temp) const
92 {
93 ScalarType chi = 0;
94 size_t size = end - begin;
95 #ifdef DOPARALLEL
96 #pragma omp parallel for schedule(static) reduction(+:chi)
97 #endif
98 for(int idx = 0; idx < size; ++idx)
99 {
100 Vec3f s;
101 for(unsigned int j = 0; j < 3; ++j)
102 s[j] = begin[idx][j] - params[j];
103 ScalarType u = params[5] * s[1] - params[4] * s[2];
104 u *= u;
105 ScalarType v = params[3] * s[2] - params[5] * s[0];
106 u += v * v;
107 v = params[4] * s[0] - params[3] * s[1];
108 u += v * v;
109 temp[idx] = std::sqrt(u);
110 chi += (values[idx] = WeightT::Weigh(temp[idx] - params[6]))
111 * values[idx];
112 }
113 return chi;
114 }
115
116 template< class IteratorT >
117 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
118 const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
119 {
120 size_t size = end - begin;
121 #ifdef DOPARALLEL
122 #pragma omp parallel for schedule(static)
123 #endif
124 for(int idx = 0; idx < size; ++idx)
125 {
126 Vec3f s;
127 for(unsigned int j = 0; j < 3; ++j)
128 s[j] = begin[idx][j] - params[j];
129 ScalarType g = s[0] * begin[idx][0] + s[1] * begin[idx][1]
130 + s[2] * begin[idx][2];
131 if(temp[idx] < 1.0e-6)
132 {
133 matrix[idx * NumParams + 0] = std::sqrt(1 - params[3] * params[3]);
134 matrix[idx * NumParams + 1] = std::sqrt(1 - params[4] * params[4]);
135 matrix[idx * NumParams + 2] = std::sqrt(1 - params[5] * params[5]);
136 }
137 else
138 {
139 matrix[idx * NumParams + 0] = (params[3] * g - s[0]) / temp[idx];
140 matrix[idx * NumParams + 1] = (params[4] * g - s[1]) / temp[idx];
141 matrix[idx * NumParams + 2] = (params[5] * g - s[2]) / temp[idx];
142 }
143 matrix[idx * NumParams + 3] = g * matrix[idx * NumParams + 0];
144 matrix[idx * NumParams + 4] = g * matrix[idx * NumParams + 1];
145 matrix[idx * NumParams + 5] = g * matrix[idx * NumParams + 2];
146 matrix[idx * NumParams + 6] = -1;
147 WeightT::template DerivWeigh< NumParams >(temp[idx] - params[6],
148 matrix + idx * NumParams);
149 }
150 }
151
152 void Normalize(ScalarType *params) const
153 {
154 ScalarType l = std::sqrt(params[3] * params[3] + params[4] * params[4]
155 + params[5] * params[5]);
156 for(unsigned int i = 3; i < 6; ++i)
157 params[i] /= l;
158 // find point on axis closest to origin
159 float lambda = -(params[0] * params[3] + params[1] * params[4] +
160 params[2] * params[5]);
161 for(unsigned int i = 0; i < 3; ++i)
162 params[i] = params[i] + lambda * params[i + 3];
163 }
164 };
165
166 private:
167 Vec3f m_axisDir;
168 Vec3f m_axisPos;
169 float m_radius;
170 GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
171 float m_angularRotatedRadians;
172 };
173
174 inline float Cylinder::Distance(const Vec3f &p) const
175 {
176 Vec3f diff = p - m_axisPos;
177 float lambda = m_axisDir.dot(diff);
178 float axisDist = (diff - lambda * m_axisDir).length();
179 return fabs(axisDist - m_radius);
180 }
181
182 inline void Cylinder::Normal(const Vec3f &p, Vec3f *normal) const
183 {
184 Vec3f diff = p - m_axisPos;
185 float lambda = m_axisDir.dot(diff);
186 *normal = diff - lambda * m_axisDir;
187 normal->normalize();
188 }
189
190 inline float Cylinder::DistanceAndNormal(const Vec3f &p, Vec3f *normal) const
191 {
192 Vec3f diff = p - m_axisPos;
193 float lambda = m_axisDir.dot(diff);
194 *normal = diff - lambda * m_axisDir;
195 float axisDist = normal->length();
196 if(axisDist > 0)
197 *normal /= axisDist;
198 return fabs(axisDist - m_radius);
199 }
200
201 inline float Cylinder::SignedDistance(const Vec3f &p) const
202 {
203 Vec3f diff = p - m_axisPos;
204 float lambda = m_axisDir.dot(diff);
205 float axisDist = (diff - lambda * m_axisDir).length();
206 return axisDist - m_radius;
207 }
208
209 template< class IteratorT >
210 bool Cylinder::LeastSquaresFit(IteratorT begin, IteratorT end)
211 {
212 float param[7];
213 for(size_t i = 0; i < 3; ++i)
214 param[i] = m_axisPos[i];
215 for(size_t i = 0; i < 3; ++i)
216 param[i + 3] = m_axisDir[i];
217 param[6] = m_radius;
218 LevMarCylinder< LevMarLSWeight > levMarCylinder;
219 if(!LevMar(begin, end, levMarCylinder, param))
220 return false;
221 for(size_t i = 0; i < 3; ++i)
222 m_axisPos[i] = param[i];
223 for(size_t i = 0; i < 3; ++i)
224 m_axisDir[i] = param[i + 3];
225 m_radius = param[6];
226 m_hcs.FromNormal(m_axisDir);
227 m_angularRotatedRadians = 0;
228 return true;
229 }
230
231 inline unsigned int Cylinder::Intersect(const Vec3f &p, const Vec3f &r,
232 float *first, float *second) const
233 {
234 using namespace std;
235 // Create a coordinate system for the cylinder. In this system, the
236 // cylinder segment center C is the origin and the cylinder axis direction
237 // W is the z-axis. U and V are the other coordinate axis directions.
238 // If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the
239 // cylinder radius. The end caps are |z| = h/2, where h is the cylinder
240 // height.
241 float fRSqr = m_radius * m_radius;
242
243 // convert incoming line origin to cylinder coordinates
244 Vec3f kDiff = p - m_axisPos;
245 Vec3f kP(kDiff.dot(m_hcs[0]), kDiff.dot(m_hcs[1]), m_axisDir.dot(kDiff));
246
247 // Get the z-value, in cylinder coordinates, of the incoming line's
248 // unit-length direction.
249 float fDz = m_axisDir.dot(r);
250
251 if(fabs(fDz) >= 1.f - 1e-7f)
252 // The line is parallel to the cylinder axis.
253 return 0;
254
255 // convert incoming line unit-length direction to cylinder coordinates
256 Vec3f kD(r.dot(m_hcs[0]), r.dot(m_hcs[1]), r.dot(m_axisDir));
257
258 float fA0, fA1, fA2, fDiscr, fRoot, fInv;
259
260 // Test intersection of line P+t*D with infinite cylinder
261 // x^2+y^2 = r^2. This reduces to computing the roots of a
262 // quadratic equation. If P = (px,py,pz) and D = (dx,dy,dz),
263 // then the quadratic equation is
264 // (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
265 fA0 = kP[0]*kP[0] + kP[1]*kP[1] - fRSqr;
266 fA1 = kP[0]*kD[0] + kP[1]*kD[1];
267 fA2 = kD[0]*kD[0] + kD[1]*kD[1];
268 fDiscr = fA1*fA1 - fA0*fA2;
269 if (fDiscr < 0)
270 // line does not intersect cylinder
271 return 0;
272 else if (fDiscr > 1e-7f)
273 {
274 // line intersects cylinder in two places
275 fRoot = sqrt(fDiscr);
276 fInv = (1.f)/fA2;
277 *first = (-fA1 - fRoot)*fInv;
278 *second = (-fA1 + fRoot)*fInv;
279 return 2;
280 }
281 // line is tangent to the cylinder
282 *first = -fA1/fA2;
283 return 1;
284 }
285
286 #endif