Cylinder.h
Go to the documentation of this file.
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  {
66  return LeastSquaresFit(pc, begin, end);
67  }
68  static bool Interpolate(const MiscLib::Vector< Cylinder >& cylinders,
69  const MiscLib::Vector< float >& weights, Cylinder* ic);
70  void Serialize(bool binary, std::ostream* o) const;
71  static size_t SerializedSize();
72  void Serialize(FILE* o) const;
73  void Serialize(float* array) const;
74  static size_t SerializedFloatSize();
75 
76  void Transform(float scale, const Vec3f& translate);
77  void Transform(const GfxTL::MatrixXX< 3, 3, float >& rot,
78  const GfxTL::Vector3Df& trans);
79  inline unsigned int Intersect(const Vec3f& p, const Vec3f& r,
80  float* first, float* second) const;
81 
82 private:
83  template< class WeightT >
84  class LevMarCylinder
85  : public WeightT
86  {
87  public:
88  enum { NumParams = 7 };
89  typedef float ScalarType;
90 
91  template< class IteratorT >
92  ScalarType Chi(const ScalarType* params, IteratorT begin, IteratorT end,
93  ScalarType* values, ScalarType* temp) const
94  {
95  ScalarType chi = 0;
96  int size = end - begin;
97  #pragma omp parallel for schedule(static) reduction(+:chi)
98  for (int idx = 0; idx < size; ++idx)
99  {
100  Vec3f s;
101  for (unsigned int j = 0; j < 3; ++j)
102  {
103  s[j] = begin[idx][j] - params[j];
104  }
105  ScalarType u = params[5] * s[1] - params[4] * s[2];
106  u *= u;
107  ScalarType v = params[3] * s[2] - params[5] * s[0];
108  u += v * v;
109  v = params[4] * s[0] - params[3] * s[1];
110  u += v * v;
111  temp[idx] = std::sqrt(u);
112  chi += (values[idx] = WeightT::Weigh(temp[idx] - params[6]))
113  * values[idx];
114  }
115  return chi;
116  }
117 
118  template< class IteratorT >
119  void Derivatives(const ScalarType* params, IteratorT begin, IteratorT end,
120  const ScalarType* values, const ScalarType* temp, ScalarType* matrix) const
121  {
122  int size = end - begin;
123  #pragma omp parallel for schedule(static)
124  for (int idx = 0; idx < size; ++idx)
125  {
126  Vec3f s;
127  for (unsigned int j = 0; j < 3; ++j)
128  {
129  s[j] = begin[idx][j] - params[j];
130  }
131  ScalarType g = s[0] * begin[idx][0] + s[1] * begin[idx][1]
132  + s[2] * begin[idx][2];
133  if (temp[idx] < 1e-6)
134  {
135  matrix[idx * NumParams + 0] = std::sqrt(1 - params[3] * params[3]);
136  matrix[idx * NumParams + 1] = std::sqrt(1 - params[4] * params[4]);
137  matrix[idx * NumParams + 2] = std::sqrt(1 - params[5] * params[5]);
138  }
139  else
140  {
141  matrix[idx * NumParams + 0] = (params[3] * g - s[0]) / temp[idx];
142  matrix[idx * NumParams + 1] = (params[4] * g - s[1]) / temp[idx];
143  matrix[idx * NumParams + 2] = (params[5] * g - s[2]) / temp[idx];
144  }
145  matrix[idx * NumParams + 3] = g * matrix[idx * NumParams + 0];
146  matrix[idx * NumParams + 4] = g * matrix[idx * NumParams + 1];
147  matrix[idx * NumParams + 5] = g * matrix[idx * NumParams + 2];
148  matrix[idx * NumParams + 6] = -1;
149  WeightT::template DerivWeigh< NumParams >(temp[idx] - params[6],
150  matrix + idx * NumParams);
151  }
152  }
153 
154  void Normalize(ScalarType* params) const
155  {
156  ScalarType l = std::sqrt(params[3] * params[3] + params[4] * params[4]
157  + params[5] * params[5]);
158  for (unsigned int i = 3; i < 6; ++i)
159  {
160  params[i] /= l;
161  }
162  // find point on axis closest to origin
163  float lambda = -(params[0] * params[3] + params[1] * params[4] +
164  params[2] * params[5]);
165  for (unsigned int i = 0; i < 3; ++i)
166  {
167  params[i] = params[i] + lambda * params[i + 3];
168  }
169  }
170  };
171 
172 private:
173  Vec3f m_axisDir;
174  Vec3f m_axisPos;
175  float m_radius;
176  GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
177  float m_angularRotatedRadians;
178 };
179 
180 inline float Cylinder::Distance(const Vec3f& p) const
181 {
182  Vec3f diff = p - m_axisPos;
183  float lambda = m_axisDir.dot(diff);
184  float axisDist = (diff - lambda * m_axisDir).length();
185  return abs(axisDist - m_radius);
186 }
187 
188 inline void Cylinder::Normal(const Vec3f& p, Vec3f* normal) const
189 {
190  Vec3f diff = p - m_axisPos;
191  float lambda = m_axisDir.dot(diff);
192  *normal = diff - lambda * m_axisDir;
193  normal->normalize();
194 }
195 
196 inline float Cylinder::DistanceAndNormal(const Vec3f& p, Vec3f* normal) const
197 {
198  Vec3f diff = p - m_axisPos;
199  float lambda = m_axisDir.dot(diff);
200  *normal = diff - lambda * m_axisDir;
201  float axisDist = normal->length();
202  if (axisDist > 0)
203  {
204  *normal /= axisDist;
205  }
206  return abs(axisDist - m_radius);
207 }
208 
209 inline float Cylinder::SignedDistance(const Vec3f& p) const
210 {
211  Vec3f diff = p - m_axisPos;
212  float lambda = m_axisDir.dot(diff);
213  float axisDist = (diff - lambda * m_axisDir).length();
214  return axisDist - m_radius;
215 }
216 
217 template< class IteratorT >
218 bool Cylinder::LeastSquaresFit(IteratorT begin, IteratorT end)
219 {
220  float param[7];
221  for (size_t i = 0; i < 3; ++i)
222  {
223  param[i] = m_axisPos[i];
224  }
225  for (size_t i = 0; i < 3; ++i)
226  {
227  param[i + 3] = m_axisDir[i];
228  }
229  param[6] = m_radius;
230  LevMarCylinder< LevMarLSWeight > levMarCylinder;
231  if (!LevMar(begin, end, levMarCylinder, param))
232  {
233  return false;
234  }
235  for (size_t i = 0; i < 3; ++i)
236  {
237  m_axisPos[i] = param[i];
238  }
239  for (size_t i = 0; i < 3; ++i)
240  {
241  m_axisDir[i] = param[i + 3];
242  }
243  m_radius = param[6];
244  m_hcs.FromNormal(m_axisDir);
245  m_angularRotatedRadians = 0;
246  return true;
247 }
248 
249 inline unsigned int Cylinder::Intersect(const Vec3f& p, const Vec3f& r,
250  float* first, float* second) const
251 {
252  using namespace std;
253  // Create a coordinate system for the cylinder. In this system, the
254  // cylinder segment center C is the origin and the cylinder axis direction
255  // W is the z-axis. U and V are the other coordinate axis directions.
256  // If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the
257  // cylinder radius. The end caps are |z| = h/2, where h is the cylinder
258  // height.
259  float fRSqr = m_radius * m_radius;
260 
261  // convert incoming line origin to cylinder coordinates
262  Vec3f kDiff = p - m_axisPos;
263  Vec3f kP(kDiff.dot(m_hcs[0]), kDiff.dot(m_hcs[1]), m_axisDir.dot(kDiff));
264 
265  // Get the z-value, in cylinder coordinates, of the incoming line's
266  // unit-length direction.
267  float fDz = m_axisDir.dot(r);
268 
269  if (abs(fDz) >= 1.f - 1e-7f)
270  // The line is parallel to the cylinder axis.
271  {
272  return 0;
273  }
274 
275  // convert incoming line unit-length direction to cylinder coordinates
276  Vec3f kD(r.dot(m_hcs[0]), r.dot(m_hcs[1]), r.dot(m_axisDir));
277 
278  float fA0, fA1, fA2, fDiscr, fRoot, fInv;
279 
280  // Test intersection of line P+t*D with infinite cylinder
281  // x^2+y^2 = r^2. This reduces to computing the roots of a
282  // quadratic equation. If P = (px,py,pz) and D = (dx,dy,dz),
283  // then the quadratic equation is
284  // (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
285  fA0 = kP[0] * kP[0] + kP[1] * kP[1] - fRSqr;
286  fA1 = kP[0] * kD[0] + kP[1] * kD[1];
287  fA2 = kD[0] * kD[0] + kD[1] * kD[1];
288  fDiscr = fA1 * fA1 - fA0 * fA2;
289  if (fDiscr < 0)
290  // line does not intersect cylinder
291  {
292  return 0;
293  }
294  else if (fDiscr > 1e-7f)
295  {
296  // line intersects cylinder in two places
297  fRoot = sqrt(fDiscr);
298  fInv = (1.f) / fA2;
299  *first = (-fA1 - fRoot) * fInv;
300  *second = (-fA1 + fRoot) * fInv;
301  return 2;
302  }
303  // line is tangent to the cylinder
304  *first = -fA1 / fA2;
305  return 1;
306 }
307 
308 #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
cyberglove_with_calib_22dof.ic
ic
Definition: cyberglove_with_calib_22dof.py:22
Cylinder::Intersect
unsigned int Intersect(const Vec3f &p, const Vec3f &r, float *first, float *second) const
Definition: Cylinder.h:249
Vector.h
Vec3f::normalize
float normalize()
Definition: basic.h:125
Vec3f
Definition: basic.h:16
Cylinder::SignedDistance
float SignedDistance(const Vec3f &p) const
Definition: Cylinder.h:209
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
LevMarFitting.h
Cylinder::Fit
bool Fit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Cylinder.h:62
Cylinder::Distance
float Distance(const Vec3f &p) const
Definition: Cylinder.h:180
NoShrinkVector.h
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
Cylinder::LeastSquaresFit
bool LeastSquaresFit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Cylinder.cpp:422
HyperplaneCoordinateSystem.h
PointCloud
Definition: PointCloud.h:69
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
Cylinder::ParallelNormalsError
Definition: Cylinder.h:23
Vec3f::dot
float dot(const Vec3f &v) const
Definition: basic.h:92
basic.h
std
Definition: Application.h:66
LevMarLSWeight.h
PointCloud.h
DLL_LINKAGE
#define DLL_LINKAGE
Definition: basic.h:11
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
Cylinder
Definition: Cylinder.h:20
Cylinder::DistanceAndNormal
float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const
Definition: Cylinder.h:196
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
Cylinder::Normal
void Normal(const Vec3f &p, Vec3f *normal) const
Definition: Cylinder.h:188