Sphere.h
Go to the documentation of this file.
1 #ifndef SPHERE_HEADER
2 #define SPHERE_HEADER
3 #include "basic.h"
4 #include <MiscLib/Vector.h>
5 #include <stdexcept>
6 #include <GfxTL/HyperplaneCoordinateSystem.h>
7 #include <utility>
8 #include "PointCloud.h"
9 #include <ostream>
10 #include <istream>
11 #include <stdio.h>
12 #include <utility>
13 #include <MiscLib/NoShrinkVector.h>
14 #include "LevMarLSWeight.h"
15 #include "LevMarFitting.h"
16 
17 #ifndef DLL_LINKAGE
18 #define DLL_LINKAGE
19 #endif
20 
21 struct DLL_LINKAGE InvalidTetrahedonError
22  : public std::runtime_error
23 {
24  InvalidTetrahedonError();
25 };
26 
27 class DLL_LINKAGE Sphere
28 {
29 public:
30  enum { RequiredSamples = 2 };
31  Sphere();
32  Sphere(const Vec3f& center, float radius);
33  Sphere(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
34  const Vec3f& p4);
35  bool Init(const MiscLib::Vector< Vec3f >& samples);
36  bool Init(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
37  const Vec3f& p4);
38  bool Init2(const Vec3f& p1, const Vec3f& p2, const Vec3f& n1,
39  const Vec3f& n2);
40  bool Init(bool binary, std::istream* i);
41  void Init(FILE* i);
42  void Init(float* array);
43  inline float Distance(const Vec3f& p) const;
44  inline void Normal(const Vec3f& p, Vec3f* normal) const;
45  inline float DistanceAndNormal(const Vec3f& p, Vec3f* normal) const;
46  inline float SignedDistance(const Vec3f& p) const;
47  void Project(const Vec3f& p, Vec3f* pp) const;
48  const Vec3f& Center() const;
49  void Center(const Vec3f& center)
50  {
51  m_center = center;
52  }
53  float Radius() const;
54  void Radius(float radius)
55  {
56  m_radius = radius;
57  }
58  bool LeastSquaresFit(const PointCloud& pc,
59  MiscLib::Vector< size_t >::const_iterator begin,
60  MiscLib::Vector< size_t >::const_iterator end);
61  template< class IteratorT >
62  bool LeastSquaresFit(IteratorT begin, IteratorT end);
63  bool Fit(const PointCloud& pc,
64  MiscLib::Vector< size_t >::const_iterator begin,
65  MiscLib::Vector< size_t >::const_iterator end)
66  {
67  return LeastSquaresFit(pc, begin, end);
68  }
69  static bool Interpolate(const MiscLib::Vector< Sphere >& spheres,
70  const MiscLib::Vector< float >& weights, Sphere* is);
71  void Serialize(bool binary, std::ostream* o) const;
72  static size_t SerializedSize();
73  void Serialize(FILE* o) const;
74  void Serialize(float* array) const;
75  static size_t SerializedFloatSize();
76 
77  void Transform(float scale, const Vec3f& translate);
78  inline unsigned int Intersect(const Vec3f& p, const Vec3f& r,
79  float* first, float* second) const;
80 
81 private:
82  template< class WeightT >
83  class LevMarSimpleSphere
84  : public WeightT
85  {
86  public:
87  enum { NumParams = 4 };
88  typedef float ScalarType;
89 
90  template< class IteratorT >
91  ScalarType Chi(const ScalarType* params, IteratorT begin, IteratorT end,
92  ScalarType* values, ScalarType* temp) const
93  {
94  ScalarType chi = 0;
95  int size = end - begin;
96  #pragma omp parallel for schedule(static) reduction(+:chi)
97  for (int idx = 0; idx < size; ++idx)
98  {
99  float s = begin[idx][0] - params[0];
100  s *= s;
101  for (unsigned int j = 1; j < 3; ++j)
102  {
103  float ss = begin[idx][j] - params[j];
104  s += ss * ss;
105  }
106  values[idx] = WeightT::Weigh(std::sqrt(s) - params[3]);
107  chi += values[idx] * values[idx];
108  }
109  return chi;
110  }
111 
112  template< class IteratorT >
113  void Derivatives(const ScalarType* params, IteratorT begin, IteratorT end,
114  const ScalarType* values, const ScalarType* temp, ScalarType* matrix) const
115  {
116  int size = end - begin;
117  #pragma omp parallel for schedule(static)
118  for (int idx = 0; idx < size; ++idx)
119  {
120  float s[3];
121  s[0] = begin[idx][0] - params[0];
122  float sl = s[0] * s[0];
123  for (unsigned int i = 1; i < 3; ++i)
124  {
125  s[i] = begin[idx][i] - params[i];
126  sl += s[i] * s[i];
127  }
128  sl = std::sqrt(sl);
129  matrix[idx * NumParams + 0] = -s[0] / sl;
130  matrix[idx * NumParams + 1] = -s[1] / sl;
131  matrix[idx * NumParams + 2] = -s[2] / sl;
132  matrix[idx * NumParams + 3] = -1;
133  WeightT::template DerivWeigh< NumParams >(sl - params[3],
134  matrix + idx * NumParams);
135  }
136  }
137 
138  void Normalize(ScalarType*) const
139  {}
140  };
141 
142  template< class WeightT >
143  class LevMarSphere
144  : public WeightT
145  {
146  public:
147  enum { NumParams = 7 };
148  typedef float ScalarType;
149 
150  // parametrization: params[0] - params[2] = normal
151  // params[3] - params[5] = point
152  // params[6] = 1 / radius
153 
154  template< class IteratorT >
155  ScalarType Chi(const ScalarType* params, IteratorT begin, IteratorT end,
156  ScalarType* values, ScalarType* temp) const
157  {
158  ScalarType chi = 0;
159  ScalarType radius = 1 / params[6];
160  Vec3f center = -radius * Vec3f(params[0], params[1], params[2])
161  + Vec3f(params[3], params[4], params[5]);
162  int size = end - begin;
163  #pragma omp parallel for schedule(static) reduction(+:chi)
164  for (int idx = 0; idx < size; ++idx)
165  {
166  temp[idx] = (begin[idx] - center).length();
167  chi += (values[idx] = WeightT::Weigh(temp[idx] - radius))
168  * values[idx];
169  }
170  return chi;
171  }
172 
173  template< class IteratorT >
174  void Derivatives(const ScalarType* params, IteratorT begin, IteratorT end,
175  const ScalarType* values, const ScalarType* temp, ScalarType* matrix) const
176  {
177  Vec3f normal(params[0], params[1], params[2]);
178  Vec3f point(params[3], params[4], params[5]);
179  int size = end - begin;
180  #pragma omp parallel for schedule(static)
181  for (int idx = 0; idx < size; ++idx)
182  {
183  ScalarType denominator = -1.f / temp[idx] * params[6];
184  matrix[idx * NumParams + 0] =
185  (matrix[idx * NumParams + 3] = (point[0] - normal[0] * params[6] - begin[idx][0]))
186  * denominator;
187  matrix[idx * NumParams + 1] =
188  (matrix[idx * NumParams + 4] = (point[1] - normal[1] * params[6] - begin[idx][1]))
189  * denominator;
190  matrix[idx * NumParams + 2] =
191  (matrix[idx * NumParams + 5] = (point[2] - normal[2] * params[6] - begin[idx][2]))
192  * denominator;
193  matrix[idx * NumParams + 3] /= temp[idx];
194  matrix[idx * NumParams + 4] /= temp[idx];
195  matrix[idx * NumParams + 5] /= temp[idx];
196  matrix[idx * NumParams + 6] = (normal[0] * matrix[idx * NumParams + 3]
197  + normal[1] * matrix[idx * NumParams + 4]
198  + normal[2] * matrix[idx * NumParams + 5] + 1) * params[6] * params[6];
199  WeightT::template DerivWeigh< NumParams >(temp[idx] - 1.f / params[6],
200  matrix + idx * NumParams);
201  }
202  }
203 
204  void Normalize(ScalarType* params) const
205  {
206  ScalarType len = std::sqrt(params[0] * params[0]
207  + params[1] * params[1] + params[2] * params[2]);
208  params[0] /= len;
209  params[1] /= len;
210  params[2] /= len;
211  }
212  };
213 
214 private:
215  Vec3f m_center;
216  float m_radius;
217 };
218 
219 inline float Sphere::Distance(const Vec3f& p) const
220 {
221  return abs((m_center - p).length() - m_radius);
222 }
223 
224 inline void Sphere::Normal(const Vec3f& p, Vec3f* normal) const
225 {
226  *normal = p - m_center;
227  normal->normalize();
228 }
229 
230 inline float Sphere::DistanceAndNormal(const Vec3f& p, Vec3f* normal) const
231 {
232  *normal = p - m_center;
233  float l = normal->length();
234  if (l > 0)
235  {
236  *normal /= l;
237  }
238  return abs(l - m_radius);
239 }
240 
241 inline float Sphere::SignedDistance(const Vec3f& p) const
242 {
243  return (m_center - p).length() - m_radius;
244 }
245 
246 template< class IteratorT >
247 bool Sphere::LeastSquaresFit(IteratorT begin, IteratorT end)
248 {
249  LevMarSimpleSphere< LevMarLSWeight > levMarSphere;
250  float param[4];
251  for (size_t i = 0; i < 3; ++i)
252  {
253  param[i] = m_center[i];
254  }
255  param[3] = m_radius;
256  if (!LevMar(begin, end, levMarSphere, param))
257  {
258  return false;
259  }
260  for (size_t i = 0; i < 3; ++i)
261  {
262  m_center[i] = param[i];
263  }
264  m_radius = param[3];
265  return true;
266 }
267 
268 inline unsigned int Sphere::Intersect(const Vec3f& p, const Vec3f& r,
269  float* first, float* second) const
270 {
271  using namespace std;
272  Vec3f kDiff = p - m_center;
273  float fA0 = kDiff.dot(kDiff) - m_radius * m_radius;
274  float fA1, fDiscr, fRoot;
275  if (fA0 <= 0)
276  {
277  // P is inside the sphere
278  fA1 = r.dot(kDiff);
279  fDiscr = fA1 * fA1 - fA0;
280  fRoot = sqrt(fDiscr);
281  *first = -fA1 + fRoot;
282  return 1;
283  }
284  // else: P is outside the sphere
285  fA1 = r.dot(kDiff);
286  if (fA1 >= 0)
287  {
288  return 0;
289  }
290  fDiscr = fA1 * fA1 - fA0;
291  if (fDiscr < 0)
292  {
293  return 0;
294  }
295  else if (fDiscr >= /* zero tolerance eps */ 1e-7f)
296  {
297  fRoot = sqrt(fDiscr);
298  *first = -fA1 - fRoot;
299  *second = -fA1 + fRoot;
300  return 2;
301  }
302  *first = -fA1;
303  return 1;
304 }
305 
306 class DLL_LINKAGE SphereAsSquaresParametrization
307 {
308 public:
309  SphereAsSquaresParametrization() {}
310  SphereAsSquaresParametrization(const Sphere& sphere,
311  const Vec3f& planeNormal);
312  void Init(const Sphere& sphere, const Vec3f& planeNormal);
313 
314  // returns < 0 if point is on lower hemisphere
315  float Parameters(const Vec3f& p,
316  std::pair< float, float >* param) const;
317  bool InSpace(const std::pair< float, float >& param, bool lower,
318  Vec3f* p) const;
319  bool InSpace(const std::pair< float, float >& param, bool lower,
320  Vec3f* p, Vec3f* n) const;
321  void Transform(const GfxTL::MatrixXX< 3, 3, float >& rot,
322  const GfxTL::Vector3Df& trans);
323  void HyperplaneCoordinateSystem(Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2) const;
324 
325 private:
326  void Hemisphere2Disk(const Vec3f& p,
327  std::pair< float, float >* inDisk) const;
328  void Disk2Square(const std::pair< float, float >& inDisk,
329  std::pair< float, float >* inSquare) const;
330  void Square2Disk(const std::pair< float, float >& inSquare,
331  std::pair< float, float >* inDisk) const;
332  void Disk2Hemisphere(const std::pair< float, float >& inDisk,
333  Vec3f* p) const;
334 
335 private:
336  Sphere m_sphere;
337  Vec3f m_planeNormal;
338  GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
339 };
340 
341 class DLL_LINKAGE UpperSphereAsSquaresParametrization
342  : public SphereAsSquaresParametrization
343 {
344 public:
345  UpperSphereAsSquaresParametrization() {}
346  UpperSphereAsSquaresParametrization(const SphereAsSquaresParametrization& p)
347  : SphereAsSquaresParametrization(p) {}
348  bool InSpace(const std::pair< float, float >& param, Vec3f* p) const
349  {
350  return SphereAsSquaresParametrization::InSpace(param, false, p);
351  }
352  bool InSpace(const std::pair< float, float >& param, Vec3f* p, Vec3f* n) const
353  {
354  return SphereAsSquaresParametrization::InSpace(param, false, p, n);
355  }
356  bool InSpace(float u, float v, Vec3f* p) const
357  {
358  return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p);
359  }
360  bool InSpace(float u, float v, Vec3f* p, Vec3f* n) const
361  {
362  return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p, n);
363  }
364 };
365 
366 class DLL_LINKAGE LowerSphereAsSquaresParametrization
367  : public SphereAsSquaresParametrization
368 {
369 public:
370  LowerSphereAsSquaresParametrization() {}
371  LowerSphereAsSquaresParametrization(const SphereAsSquaresParametrization& p)
372  : SphereAsSquaresParametrization(p) {}
373  bool InSpace(const std::pair< float, float >& param, Vec3f* p) const
374  {
375  return SphereAsSquaresParametrization::InSpace(param, true, p);
376  }
377  bool InSpace(const std::pair< float, float >& param, Vec3f* p, Vec3f* n) const
378  {
379  return SphereAsSquaresParametrization::InSpace(param, true, p, n);
380  }
381  bool InSpace(float u, float v, Vec3f* p) const
382  {
383  return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p);
384  }
385  bool InSpace(float u, float v, Vec3f* p, Vec3f* n) const
386  {
387  return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p, n);
388  }
389 };
390 
391 #endif
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
GfxTL::HyperplaneCoordinateSystem< float, 3 >
GfxTL::VectorXD
Definition: MatrixXX.h:21
UpperSphereAsSquaresParametrization::UpperSphereAsSquaresParametrization
UpperSphereAsSquaresParametrization()
Definition: Sphere.h:345
Vector.h
Vec3f::normalize
float normalize()
Definition: basic.h:125
Vec3f
Definition: basic.h:16
SphereAsSquaresParametrization
Definition: Sphere.h:306
LowerSphereAsSquaresParametrization::LowerSphereAsSquaresParametrization
LowerSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p)
Definition: Sphere.h:371
Sphere::Distance
float Distance(const Vec3f &p) const
Definition: Sphere.h:219
UpperSphereAsSquaresParametrization::InSpace
bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const
Definition: Sphere.h:360
UpperSphereAsSquaresParametrization::UpperSphereAsSquaresParametrization
UpperSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p)
Definition: Sphere.h:346
SphereAsSquaresParametrization::InSpace
bool InSpace(const std::pair< float, float > &param, bool lower, Vec3f *p) const
Definition: Sphere.cpp:494
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
Vec3f::length
float length() const
Definition: basic.h:120
GfxTL::MatrixXX
Definition: MatrixXX.h:25
MiscLib::Vector
Definition: Vector.h:19
LowerSphereAsSquaresParametrization::InSpace
bool InSpace(const std::pair< float, float > &param, Vec3f *p) const
Definition: Sphere.h:373
LowerSphereAsSquaresParametrization::LowerSphereAsSquaresParametrization
LowerSphereAsSquaresParametrization()
Definition: Sphere.h:370
LevMarFitting.h
LowerSphereAsSquaresParametrization::InSpace
bool InSpace(const std::pair< float, float > &param, Vec3f *p, Vec3f *n) const
Definition: Sphere.h:377
InvalidTetrahedonError
Definition: Sphere.h:21
UpperSphereAsSquaresParametrization
Definition: Sphere.h:341
Sphere::Fit
bool Fit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Sphere.h:63
NoShrinkVector.h
UpperSphereAsSquaresParametrization::InSpace
bool InSpace(const std::pair< float, float > &param, Vec3f *p) const
Definition: Sphere.h:348
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
UpperSphereAsSquaresParametrization::InSpace
bool InSpace(const std::pair< float, float > &param, Vec3f *p, Vec3f *n) const
Definition: Sphere.h:352
LowerSphereAsSquaresParametrization
Definition: Sphere.h:366
Sphere::Intersect
unsigned int Intersect(const Vec3f &p, const Vec3f &r, float *first, float *second) const
Definition: Sphere.h:268
Sphere::Center
void Center(const Vec3f &center)
Definition: Sphere.h:49
HyperplaneCoordinateSystem.h
LowerSphereAsSquaresParametrization::InSpace
bool InSpace(float u, float v, Vec3f *p) const
Definition: Sphere.h:381
LowerSphereAsSquaresParametrization::InSpace
bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const
Definition: Sphere.h:385
Sphere::Normal
void Normal(const Vec3f &p, Vec3f *normal) const
Definition: Sphere.h:224
Sphere::Radius
void Radius(float radius)
Definition: Sphere.h:54
Sphere::Init
bool Init(const MiscLib::Vector< Vec3f > &samples)
Definition: Sphere.cpp:172
SphereAsSquaresParametrization::SphereAsSquaresParametrization
SphereAsSquaresParametrization()
Definition: Sphere.h:309
PointCloud
Definition: PointCloud.h:69
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
Sphere::DistanceAndNormal
float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const
Definition: Sphere.h:230
Vec3f::dot
float dot(const Vec3f &v) const
Definition: basic.h:92
basic.h
std
Definition: Application.h:66
UpperSphereAsSquaresParametrization::InSpace
bool InSpace(float u, float v, Vec3f *p) const
Definition: Sphere.h:356
LevMarLSWeight.h
PointCloud.h
DLL_LINKAGE
#define DLL_LINKAGE
Definition: basic.h:11
Sphere
Definition: Sphere.h:27
GfxTL::Vec3f
VectorXD< 3, float > Vec3f
Definition: VectorXD.h:691
Sphere::LeastSquaresFit
bool LeastSquaresFit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Sphere.cpp:389
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
LevMar
bool LevMar(IteratorT begin, IteratorT end, FuncT &func, typename FuncT::ScalarType *param)
Definition: LevMarFitting.h:143
Sphere::Transform
void Transform(float scale, const Vec3f &translate)
Definition: Sphere.cpp:454
Sphere::SignedDistance
float SignedDistance(const Vec3f &p) const
Definition: Sphere.h:241