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