Debian Codegraph @

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

raw
1 #include "Cylinder.h"
2 #include "LevMarFitting.h"
3 #include <MiscLib/Performance.h>
4 #include <GfxTL/VectorXD.h>
5 #include "LevMarFitting.h"
6 #include "LevMarLSWeight.h"
7 #include <GfxTL/IndexedIterator.h>
8 #include <GfxTL/Mean.h>
9 #ifdef DOPARALLEL
10 #include <omp.h>
11 #endif
12
13 Cylinder::ParallelNormalsError::ParallelNormalsError()
14 : std::runtime_error("Parallel normals")
15 {}
16
17 Cylinder::Cylinder()
18 : m_angularRotatedRadians(0)
19 , m_radius(0.0f)
20 {}
21
22 Cylinder::Cylinder(const Vec3f &axisDir, const Vec3f &axisPos, float radius)
23 : m_angularRotatedRadians(0)
24 {
25 Init(axisDir, axisPos, radius);
26 }
27
28 Cylinder::Cylinder(const Vec3f &pointA, const Vec3f &pointB,
29 const Vec3f &normalA, const Vec3f &normalB)
30 : m_angularRotatedRadians(0)
31 {
32 if(!Init(pointA, pointB, normalA, normalB))
33 throw ParallelNormalsError();
34 }
35
36 bool Cylinder::Init(const MiscLib::Vector< Vec3f > &samples)
37 {
38 if(samples.size() < 4)
39 return false;
40 // estimate axis from all pairs
41 m_axisDir = Vec3f(0, 0, 0);
42 size_t c = samples.size() / 2;
43 m_axisDir = samples[0 + c].cross(samples[1 + c]);
44 if(m_axisDir.normalize() < 1e-3)
45 return false;
46 m_axisPos = Vec3f(0, 0, 0);
47 m_radius = 0;
48 // project first normal into plane
49 float l = m_axisDir.dot(samples[0 + c]);
50 Vec3f xdir = samples[0 + c] - l * m_axisDir;
51 xdir.normalize();
52 Vec3f ydir = m_axisDir.cross(xdir);
53 ydir.normalize();
54 // xdir is the x axis in the plane (y = 0) samples[0] is the origin
55 float lineBnx = ydir.dot(samples[1 + c]);
56 if(fabs(lineBnx) < 1.0e-6)
57 return false;
58 float lineBny = -xdir.dot(samples[1 + c]);
59 // origin of lineB
60 Vec3f originB = samples[1] - samples[0];
61 float lineBOx = xdir.dot(originB);
62 float lineBOy = ydir.dot(originB);
63 float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
64 // lineB in the plane complete
65 // point of intersection is y = 0 and x = lineBd / lineBnx
66 float radius = lineBd / lineBnx;
67 m_axisPos += samples[0] + radius * xdir;
68 m_radius += fabs(radius);
69 m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
70 m_radius /= 2;
71 if(m_radius > 1e6)
72 return false;
73
74 // find point on axis closest to origin
75 float lambda = m_axisDir.dot(-m_axisPos);
76 m_axisPos = m_axisPos + lambda * m_axisDir;
77
78 m_hcs.FromNormal(m_axisDir);
79 m_angularRotatedRadians = 0;
80 return true;
81 }
82
83 bool Cylinder::InitAverage(const MiscLib::Vector< Vec3f > &samples)
84 {
85 if(samples.size() < 4)
86 return false;
87 // estimate axis from covariance of normal vectors
88 MiscLib::Vector< GfxTL::Vector3Df > normals;
89 size_t c = samples.size() / 2;
90 for(size_t i = c; i < samples.size(); ++i)
91 {
92 normals.push_back(GfxTL::Vector3Df(samples[i]));
93 normals.push_back(GfxTL::Vector3Df(-samples[i]));
94 }
95 GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
96 GfxTL::Vector3Df eigenValues;
97 GfxTL::CovarianceMatrix(GfxTL::Vector3Df(0, 0, 0),
98 normals.begin(), normals.end(), &cov);
99 GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
100 // find the minimal eigenvalue and corresponding vector
101 float minEigVal = eigenValues[0];
102 unsigned int minEigIdx = 0;
103 for(unsigned int i = 1; i < 3; ++i)
104 if(eigenValues[i] < minEigVal)
105 {
106 minEigVal = eigenValues[i];
107 minEigIdx = i;
108 }
109 m_axisDir = Vec3f(eigenVectors[minEigIdx]);
110 // get a point on the axis from all pairs
111 m_axisPos = Vec3f(0, 0, 0);
112 m_radius = 0;
113 size_t pointCount = 0;
114 for(size_t i = 0; i < c - 1; ++i)
115 for(size_t j = i + 1; j < c; ++j)
116 {
117 // project first normal into plane
118 float l = m_axisDir.dot(samples[i + c]);
119 Vec3f xdir = samples[i + c] - l * m_axisDir;
120 xdir.normalize();
121 Vec3f ydir = m_axisDir.cross(xdir);
122 ydir.normalize();
123 // xdir is the x axis in the plane (y = 0) samples[i] is the origin
124 float lineBnx = ydir.dot(samples[j + c]);
125 if(fabs(lineBnx) < .05f)
126 continue;
127 float lineBny = -xdir.dot(samples[j + c]);
128 // origin of lineB
129 Vec3f originB = samples[j] - samples[i];
130 float lineBOx = xdir.dot(originB);
131 float lineBOy = ydir.dot(originB);
132 float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
133 // lineB in the plane complete
134 // point of intersection is y = 0 and x = lineBd / lineBnx
135 float radius = lineBd / lineBnx;
136 m_axisPos += samples[i] + radius * xdir;
137 m_radius += fabs(radius);
138 m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
139 ++pointCount;
140 }
141 if(!pointCount)
142 return false;
143 m_axisPos /= static_cast<float>(pointCount);
144 m_radius /= (pointCount * 2);
145 if(m_radius > 1e6)
146 return false;
147
148 // find point on axis closest to origin
149 float lambda = m_axisDir.dot(-m_axisPos);
150 m_axisPos = m_axisPos + lambda * m_axisDir;
151
152 m_hcs.FromNormal(m_axisDir);
153 m_angularRotatedRadians = 0;
154 return true;
155 }
156
157 bool Cylinder::Init(const Vec3f &axisDir, const Vec3f &axisPos, float radius)
158 {
159 m_axisDir = axisDir;
160 m_axisPos = axisPos;
161 m_radius = radius;
162 m_hcs.FromNormal(m_axisDir);
163 m_angularRotatedRadians = 0;
164 return true;
165 }
166
167 bool Cylinder::Init(const Vec3f &pointA, const Vec3f &pointB,
168 const Vec3f &normalA, const Vec3f &normalB)
169 {
170 if(normalA.dot(normalB) > 0.9998477)
171 return false;
172 m_axisDir = normalA.cross(normalB);
173 if(m_axisDir.normalize() < 1.0e-6)
174 return false;
175 // normalA is the x axis in the plane (y = 0) pointA is the origin
176 Vec3f planeY = normalA.cross(m_axisDir); // planeX = normalA
177 planeY.normalize();
178 float lineBnx = planeY.dot(normalB);
179 float lineBny = -normalA.dot(normalB);
180 // origin of lineB
181 Vec3f originB = pointB - pointA;
182 float lineBOx = normalA.dot(originB);
183 float lineBOy = planeY.dot(originB);
184 float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
185 // lineB in the plane complete
186 // point of intersection is y = 0 and x = lineBd / lineBnx
187 m_radius = lineBd / lineBnx;
188 m_axisPos = pointA + m_radius * normalA;
189 m_radius = fabs(m_radius);
190 if(m_radius > 1e6)
191 return false;
192 m_hcs.FromNormal(m_axisDir);
193 m_angularRotatedRadians = 0;
194 return true;
195 }
196
197 bool Cylinder::Init(bool binary, std::istream *i)
198 {
199 float rotate = 0;
200 if(binary)
201 {
202 i->read((char *)&m_axisDir, sizeof(m_axisDir));
203 i->read((char *)&m_axisPos, sizeof(m_axisPos));
204 i->read((char *)&m_radius, sizeof(m_radius));
205 i->read((char *)&rotate, sizeof(rotate));
206 }
207 else
208 {
209 for(size_t j = 0; j < 3; ++j)
210 (*i) >> m_axisDir[j];
211 for(size_t j = 0; j < 3; ++j)
212 (*i) >> m_axisPos[j];
213 (*i) >> m_radius;
214 (*i) >> rotate;
215 }
216 m_hcs.FromNormal(m_axisDir);
217 m_angularRotatedRadians = 0;
218 RotateAngularDirection(rotate);
219 return true;
220 }
221
222 void Cylinder::Init(FILE *i)
223 {
224 float rotate = 0;
225 size_t readrtn; //unused return warning suppresion
226 readrtn = fread(&m_axisDir, sizeof(m_axisDir), 1, i);
227 readrtn = fread(&m_axisPos, sizeof(m_axisPos), 1, i);
228 readrtn = fread(&m_radius, sizeof(m_radius), 1, i);
229 readrtn = fread(&rotate, sizeof(rotate), 1, i);
230 m_hcs.FromNormal(m_axisDir);
231 m_angularRotatedRadians = 0;
232 RotateAngularDirection(rotate);
233 }
234
235 void Cylinder::Init(float *array)
236 {
237 float rotate = 0;
238 for(int i=0;i<3;i++){
239 m_axisDir[i] = array[i];
240 m_axisPos[i] = array[i+3];
241 }
242 m_radius=array[6];
243 rotate=array[7];
244
245 m_hcs.FromNormal(m_axisDir);
246 m_angularRotatedRadians = 0;
247 RotateAngularDirection(rotate);
248 }
249
250 void Cylinder::Project(const Vec3f &p, Vec3f *pp) const
251 {
252 Vec3f diff = m_axisPos - p;
253 float lambda = m_axisDir.dot(diff);
254 *pp = (diff - lambda * m_axisDir);
255 float l = pp->length();
256 *pp *= (l - m_radius) / l;
257 *pp += p;
258 }
259
260 void Cylinder::Parameters(const Vec3f &p,
261 std::pair< float, float > *param) const
262 {
263 Vec3f diff = p - m_axisPos;
264 param->first = m_axisDir.dot(diff);
265 float planex = diff.dot(m_hcs[0].Data());
266 float planey = diff.dot(m_hcs[1].Data());
267 float l = planex * planex + planey * planey;
268 if(l > 0)
269 {
270 planex /= l;
271 planey /= l;
272 }
273 param->second = std::atan2(planey, planex);
274 if(param->second < 0)
275 param->second += float(2 * M_PI);
276 }
277
278 float Cylinder::Radius() const
279 {
280 return m_radius;
281 }
282
283 float &Cylinder::Radius()
284 {
285 return m_radius;
286 }
287
288 const Vec3f &Cylinder::AxisDirection() const
289 {
290 return m_axisDir;
291 }
292
293 Vec3f &Cylinder::AxisDirection()
294 {
295 return m_axisDir;
296 }
297
298 const Vec3f &Cylinder::AxisPosition() const
299 {
300 return m_axisPos;
301 }
302
303 Vec3f &Cylinder::AxisPosition()
304 {
305 return m_axisPos;
306 }
307
308 const Vec3f Cylinder::AngularDirection() const
309 {
310 return Vec3f(m_hcs[0].Data());
311 }
312
313 void Cylinder::RotateAngularDirection(float radians)
314 {
315 GfxTL::Quaternion< float > q;
316 q.RotationRad(radians, m_axisDir[0], m_axisDir[1], m_axisDir[2]);
317 Vec3f vvec;
318 q.Rotate(AngularDirection(), &vvec);
319 m_hcs[0] = GfxTL::Vector3Df(vvec);
320 m_hcs[1] = GfxTL::Vector3Df(m_axisDir.cross(Vec3f(m_hcs[0].Data())));
321 m_angularRotatedRadians += radians;
322 }
323
324 float CylinderDistance(const float *param, const float *x)
325 {
326 Vec3f s;
327 for(size_t i = 0; i < 3; ++i)
328 s[i] = x[i] - param[i];
329 float u = param[5] * s[1] - param[4] * s[2];
330 u *= u;
331 float v = param[3] * s[2] - param[5] * s[0];
332 u += v * v;
333 v = param[4] * s[0] - param[3] * s[1];
334 u += v * v;
335 return std::sqrt(u) - param[6];
336 }
337
338 void CylinderDistanceDerivatives(const float *param, const float *x,
339 float *gradient)
340 {
341 Vec3f s;
342 for(size_t i = 0; i < 3; ++i)
343 s[i] = x[i] - param[i];
344 float g = s[0] * x[0] + s[1] * x[1] + s[2] * x[2];
345 float f = param[5] * s[1] - param[4] * s[2];
346 f *= f;
347 float v = param[3] * s[2] - param[5] * s[0];
348 f += v * v;
349 v = param[4] * s[0] - param[3] * s[1];
350 f += v * v;
351 f = std::sqrt(f);
352 if(f < 1.0e-6)
353 {
354 gradient[0] = std::sqrt(1 - param[3] * param[3]);
355 gradient[1] = std::sqrt(1 - param[4] * param[4]);
356 gradient[2] = std::sqrt(1 - param[5] * param[5]);
357 }
358 else
359 {
360 gradient[0] = (param[3] * g - s[0]) / f;
361 gradient[1] = (param[4] * g - s[1]) / f;
362 gradient[2] = (param[5] * g - s[2]) / f;
363 }
364 gradient[3] = g * gradient[0];
365 gradient[4] = g * gradient[1];
366 gradient[5] = g * gradient[2];
367 gradient[6] = -1;
368 }
369
370 void NormalizeCylinderParams(float *param)
371 {
372 float l = std::sqrt(param[3] * param[3] + param[4] * param[4]
373 + param[5] * param[5]);
374 for(unsigned int i = 3; i < 6; ++i)
375 param[i] /= l;
376 // find point on axis closest to origin
377 float lambda = -(param[0] * param[3] + param[1] * param[4] +
378 param[2] * param[5]);
379 for(unsigned int i = 0; i < 3; ++i)
380 param[i] = param[i] + lambda * param[i + 3];
381 }
382
383 bool Cylinder::LeastSquaresFit(const PointCloud &pc,
384 MiscLib::Vector< size_t >::const_iterator begin,
385 MiscLib::Vector< size_t >::const_iterator end)
386 {
387 bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
388 GfxTL::IndexIterate(end, pc.begin()));
389 return retVal;
390 }
391
392 bool Cylinder::Interpolate(const MiscLib::Vector< Cylinder > &cylinders,
393 const MiscLib::Vector< float > &weights, Cylinder *ic)
394 {
395 Vec3f axisPos(0, 0, 0);
396 Vec3f axisDir(0, 0, 0);
397 float r = 0;
398 for(size_t i = 0; i < cylinders.size(); ++i)
399 {
400 axisPos += weights[i] * cylinders[i].AxisPosition();
401 axisDir += weights[i] * cylinders[i].AxisDirection();
402 r += weights[i] * cylinders[i].Radius();
403 }
404 axisDir.normalize();
405 return ic->Init(axisDir, axisPos, r);
406 }
407
408 void Cylinder::Serialize(bool binary, std::ostream *o) const
409 {
410 if(binary)
411 {
412 o->write((const char *)&m_axisDir, sizeof(m_axisDir));
413 o->write((const char *)&m_axisPos, sizeof(m_axisPos));
414 o->write((const char *)&m_radius, sizeof(m_radius));
415 o->write((const char *)&m_angularRotatedRadians,
416 sizeof(m_angularRotatedRadians));
417 }
418 else
419 {
420 (*o) << m_axisDir[0] << " " << m_axisDir[1] << " " << m_axisDir[2] << " "
421 << m_axisPos[0] << " " << m_axisPos[1] << " " << m_axisPos[2] << " "
422 << m_radius << " " << m_angularRotatedRadians << " ";
423 }
424 }
425
426 size_t Cylinder::SerializedSize()
427 {
428 return sizeof(Vec3f)
429 + sizeof(Vec3f)
430 + sizeof(float)
431 + sizeof(float);
432 }
433
434 void Cylinder::Serialize(float *array) const
435 {
436
437 for(int i=0;i<3;i++){
438 array[i] = m_axisDir[i];
439 array[i+3] = m_axisPos[i];
440 }
441 array[6] = m_radius;
442 array[7] = m_angularRotatedRadians;
443 }
444
445 size_t Cylinder::SerializedFloatSize()
446 {
447 return 8;
448 }
449
450 void Cylinder::Serialize(FILE *o) const
451 {
452 fwrite(&m_axisDir, sizeof(m_axisDir), 1, o);
453 fwrite(&m_axisPos, sizeof(m_axisPos), 1, o);
454 fwrite(&m_radius, sizeof(m_radius), 1, o);
455 fwrite(&m_angularRotatedRadians,
456 sizeof(m_angularRotatedRadians), 1, o);
457 }
458
459 void Cylinder::Transform(float scale, const Vec3f &translate)
460 {
461 m_axisPos *= scale;
462 m_axisPos += translate;
463 m_radius *= scale;
464 }
465
466 void Cylinder::Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
467 const GfxTL::Vector3Df &trans)
468 {
469 m_axisDir = Vec3f((rot * GfxTL::Vector3Df(m_axisDir)).Data());
470 m_axisPos = Vec3f((rot * GfxTL::Vector3Df(m_axisPos) + trans).Data());
471 m_hcs[0] = rot * m_hcs[0];
472 m_hcs[1] = rot * m_hcs[1];
473 }