Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
eigen.hpp
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
35 */
36 
37 // This file is part of Eigen, a lightweight C++ template library
38 // for linear algebra.
39 //
40 // Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
41 //
42 // Eigen is free software; you can redistribute it and/or
43 // modify it under the terms of the GNU Lesser General Public
44 // License as published by the Free Software Foundation; either
45 // version 3 of the License, or (at your option) any later version.
46 //
47 // Alternatively, you can redistribute it and/or
48 // modify it under the terms of the GNU General Public License as
49 // published by the Free Software Foundation; either version 2 of
50 // the License, or (at your option) any later version.
51 //
52 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
53 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
54 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
55 // GNU General Public License for more details.
56 //
57 // You should have received a copy of the GNU Lesser General Public
58 // License and a copy of the GNU General Public License along with
59 // Eigen. If not, see <http://www.gnu.org/licenses/>.
60 
61 // The computeRoots function included in this is based on materials
62 // covered by the following copyright and license:
63 //
64 // Geometric Tools, LLC
65 // Copyright (c) 1998-2010
66 // Distributed under the Boost Software License, Version 1.0.
67 //
68 // Permission is hereby granted, free of charge, to any person or organization
69 // obtaining a copy of the software and accompanying documentation covered by
70 // this license (the "Software") to use, reproduce, display, distribute,
71 // execute, and transmit the Software, and to prepare derivative works of the
72 // Software, and to permit third-parties to whom the Software is furnished to
73 // do so, all subject to the following:
74 //
75 // The copyright notices in the Software and this entire statement, including
76 // the above license grant, this restriction and the following disclaimer,
77 // must be included in all copies of the Software, in whole or in part, and
78 // all derivative works of the Software, unless such copies or derivative
79 // works are solely in the form of machine-executable object code generated by
80 // a source language processor.
81 //
82 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
83 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
84 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
85 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
86 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
87 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
88 // DEALINGS IN THE SOFTWARE.
89 
90 #ifndef PCL_GPU_FEATURES_EIGEN_HPP_
91 #define PCL_GPU_FEATURES_EIGEN_HPP_
92 
93 #include <pcl/gpu/utils/device/limits.hpp>
94 #include <pcl/gpu/utils/device/algorithm.hpp>
95 #include <pcl/gpu/utils/device/vector_math.hpp>
96 
97 namespace pcl
98 {
99  namespace device
100  {
101  __device__ __forceinline__ void computeRoots2(const float& b, const float& c, float3& roots)
102  {
103  roots.x = 0.f;
104  float d = b * b - 4.f * c;
105  if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
106  d = 0.f;
107 
108  float sd = sqrtf(d);
109 
110  roots.z = 0.5f * (b + sd);
111  roots.y = 0.5f * (b - sd);
112  }
113 
114  __device__ __forceinline__ void computeRoots3(float c0, float c1, float c2, float3& roots)
115  {
116  if ( fabsf(c0) < numeric_limits<float>::epsilon())// one root is 0 -> quadratic equation
117  {
118  computeRoots2 (c2, c1, roots);
119  }
120  else
121  {
122  const float s_inv3 = 1.f/3.f;
123  const float s_sqrt3 = sqrtf(3.f);
124  // Construct the parameters used in classifying the roots of the equation
125  // and in solving the equation for the roots in closed form.
126  float c2_over_3 = c2 * s_inv3;
127  float a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
128  if (a_over_3 > 0.f)
129  a_over_3 = 0.f;
130 
131  float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
132 
133  float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
134  if (q > 0.f)
135  q = 0.f;
136 
137  // Compute the eigenvalues by solving for the roots of the polynomial.
138  float rho = sqrtf(-a_over_3);
139  float theta = atan2f (sqrtf (-q), half_b)*s_inv3;
140  float cos_theta = __cosf (theta);
141  float sin_theta = __sinf (theta);
142  roots.x = c2_over_3 + 2.f * rho * cos_theta;
143  roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
144  roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
145 
146  // Sort in increasing order.
147  if (roots.x >= roots.y)
148  swap(roots.x, roots.y);
149 
150  if (roots.y >= roots.z)
151  {
152  swap(roots.y, roots.z);
153 
154  if (roots.x >= roots.y)
155  swap (roots.x, roots.y);
156  }
157  if (roots.x <= 0) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
158  computeRoots2 (c2, c1, roots);
159  }
160  }
161 
162  struct Eigen33
163  {
164  public:
165  template<int Rows>
166  struct MiniMat
167  {
168  float3 data[Rows];
169  __device__ __host__ __forceinline__ float3& operator[](int i) { return data[i]; }
170  __device__ __host__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
171  };
172  typedef MiniMat<3> Mat33;
173  typedef MiniMat<4> Mat43;
174 
175 
176  static __forceinline__ __device__ float3 unitOrthogonal (const float3& src)
177  {
178  float3 perp;
179  /* Let us compute the crossed product of *this with a vector
180  * that is not too close to being colinear to *this.
181  */
182 
183  /* unless the x and y coords are both close to zero, we can
184  * simply take ( -y, x, 0 ) and normalize it.
185  */
186  if(!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
187  {
188  float invnm = rsqrtf(src.x*src.x + src.y*src.y);
189  perp.x = -src.y * invnm;
190  perp.y = src.x * invnm;
191  perp.z = 0.0f;
192  }
193  /* if both x and y are close to zero, then the vector is close
194  * to the z-axis, so it's far from colinear to the x-axis for instance.
195  * So we take the crossed product with (1,0,0) and normalize it.
196  */
197  else
198  {
199  float invnm = rsqrtf(src.z * src.z + src.y * src.y);
200  perp.x = 0.0f;
201  perp.y = -src.z * invnm;
202  perp.z = src.y * invnm;
203  }
204 
205  return perp;
206  }
207 
208  __device__ __forceinline__ Eigen33(volatile float* mat_pkg_arg) : mat_pkg(mat_pkg_arg) {}
209  __device__ __forceinline__ void compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals)
210  {
211  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
212  // only when at least one matrix entry has magnitude larger than 1.
213 
214  float max01 = fmaxf( fabsf(mat_pkg[0]), fabsf(mat_pkg[1]) );
215  float max23 = fmaxf( fabsf(mat_pkg[2]), fabsf(mat_pkg[3]) );
216  float max45 = fmaxf( fabsf(mat_pkg[4]), fabsf(mat_pkg[5]) );
217  float m0123 = fmaxf( max01, max23);
218  float scale = fmaxf( max45, m0123);
219 
220  if (scale <= numeric_limits<float>::min())
221  scale = 1.f;
222 
223  mat_pkg[0] /= scale;
224  mat_pkg[1] /= scale;
225  mat_pkg[2] /= scale;
226  mat_pkg[3] /= scale;
227  mat_pkg[4] /= scale;
228  mat_pkg[5] /= scale;
229 
230  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
231  // eigenvalues are the roots to this equation, all guaranteed to be
232  // real-valued, because the matrix is symmetric.
233  float c0 = m00() * m11() * m22()
234  + 2.f * m01() * m02() * m12()
235  - m00() * m12() * m12()
236  - m11() * m02() * m02()
237  - m22() * m01() * m01();
238  float c1 = m00() * m11() -
239  m01() * m01() +
240  m00() * m22() -
241  m02() * m02() +
242  m11() * m22() -
243  m12() * m12();
244  float c2 = m00() + m11() + m22();
245 
246  computeRoots3(c0, c1, c2, evals);
247 
248  if(evals.z - evals.x <= numeric_limits<float>::epsilon())
249  {
250  evecs[0] = make_float3(1.f, 0.f, 0.f);
251  evecs[1] = make_float3(0.f, 1.f, 0.f);
252  evecs[2] = make_float3(0.f, 0.f, 1.f);
253  }
254  else if (evals.y - evals.x <= numeric_limits<float>::epsilon() )
255  {
256  // first and second equal
257  tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
258  tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
259 
260  vec_tmp[0] = cross(tmp[0], tmp[1]);
261  vec_tmp[1] = cross(tmp[0], tmp[2]);
262  vec_tmp[2] = cross(tmp[1], tmp[2]);
263 
264  float len1 = dot (vec_tmp[0], vec_tmp[0]);
265  float len2 = dot (vec_tmp[1], vec_tmp[1]);
266  float len3 = dot (vec_tmp[2], vec_tmp[2]);
267 
268  if (len1 >= len2 && len1 >= len3)
269  {
270  evecs[2] = vec_tmp[0] * rsqrtf (len1);
271  }
272  else if (len2 >= len1 && len2 >= len3)
273  {
274  evecs[2] = vec_tmp[1] * rsqrtf (len2);
275  }
276  else
277  {
278  evecs[2] = vec_tmp[2] * rsqrtf (len3);
279  }
280 
281  evecs[1] = unitOrthogonal(evecs[2]);
282  evecs[0] = cross(evecs[1], evecs[2]);
283  }
284  else if (evals.z - evals.y <= numeric_limits<float>::epsilon() )
285  {
286  // second and third equal
287  tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
288  tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
289 
290  vec_tmp[0] = cross(tmp[0], tmp[1]);
291  vec_tmp[1] = cross(tmp[0], tmp[2]);
292  vec_tmp[2] = cross(tmp[1], tmp[2]);
293 
294  float len1 = dot(vec_tmp[0], vec_tmp[0]);
295  float len2 = dot(vec_tmp[1], vec_tmp[1]);
296  float len3 = dot(vec_tmp[2], vec_tmp[2]);
297 
298  if (len1 >= len2 && len1 >= len3)
299  {
300  evecs[0] = vec_tmp[0] * rsqrtf(len1);
301  }
302  else if (len2 >= len1 && len2 >= len3)
303  {
304  evecs[0] = vec_tmp[1] * rsqrtf(len2);
305  }
306  else
307  {
308  evecs[0] = vec_tmp[2] * rsqrtf(len3);
309  }
310 
311  evecs[1] = unitOrthogonal( evecs[0] );
312  evecs[2] = cross(evecs[0], evecs[1]);
313  }
314  else
315  {
316 
317  tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
318  tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
319 
320  vec_tmp[0] = cross(tmp[0], tmp[1]);
321  vec_tmp[1] = cross(tmp[0], tmp[2]);
322  vec_tmp[2] = cross(tmp[1], tmp[2]);
323 
324  float len1 = dot(vec_tmp[0], vec_tmp[0]);
325  float len2 = dot(vec_tmp[1], vec_tmp[1]);
326  float len3 = dot(vec_tmp[2], vec_tmp[2]);
327 
328  float mmax[3];
329 
330  unsigned int min_el = 2;
331  unsigned int max_el = 2;
332  if (len1 >= len2 && len1 >= len3)
333  {
334  mmax[2] = len1;
335  evecs[2] = vec_tmp[0] * rsqrtf (len1);
336  }
337  else if (len2 >= len1 && len2 >= len3)
338  {
339  mmax[2] = len2;
340  evecs[2] = vec_tmp[1] * rsqrtf (len2);
341  }
342  else
343  {
344  mmax[2] = len3;
345  evecs[2] = vec_tmp[2] * rsqrtf (len3);
346  }
347 
348  tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
349  tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
350 
351  vec_tmp[0] = cross(tmp[0], tmp[1]);
352  vec_tmp[1] = cross(tmp[0], tmp[2]);
353  vec_tmp[2] = cross(tmp[1], tmp[2]);
354 
355  len1 = dot(vec_tmp[0], vec_tmp[0]);
356  len2 = dot(vec_tmp[1], vec_tmp[1]);
357  len3 = dot(vec_tmp[2], vec_tmp[2]);
358 
359  if (len1 >= len2 && len1 >= len3)
360  {
361  mmax[1] = len1;
362  evecs[1] = vec_tmp[0] * rsqrtf (len1);
363  min_el = len1 <= mmax[min_el] ? 1 : min_el;
364  max_el = len1 > mmax[max_el] ? 1 : max_el;
365  }
366  else if (len2 >= len1 && len2 >= len3)
367  {
368  mmax[1] = len2;
369  evecs[1] = vec_tmp[1] * rsqrtf (len2);
370  min_el = len2 <= mmax[min_el] ? 1 : min_el;
371  max_el = len2 > mmax[max_el] ? 1 : max_el;
372  }
373  else
374  {
375  mmax[1] = len3;
376  evecs[1] = vec_tmp[2] * rsqrtf (len3);
377  min_el = len3 <= mmax[min_el] ? 1 : min_el;
378  max_el = len3 > mmax[max_el] ? 1 : max_el;
379  }
380 
381  tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
382  tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
383 
384  vec_tmp[0] = cross(tmp[0], tmp[1]);
385  vec_tmp[1] = cross(tmp[0], tmp[2]);
386  vec_tmp[2] = cross(tmp[1], tmp[2]);
387 
388  len1 = dot (vec_tmp[0], vec_tmp[0]);
389  len2 = dot (vec_tmp[1], vec_tmp[1]);
390  len3 = dot (vec_tmp[2], vec_tmp[2]);
391 
392 
393  if (len1 >= len2 && len1 >= len3)
394  {
395  mmax[0] = len1;
396  evecs[0] = vec_tmp[0] * rsqrtf (len1);
397  min_el = len3 <= mmax[min_el] ? 0 : min_el;
398  max_el = len3 > mmax[max_el] ? 0 : max_el;
399  }
400  else if (len2 >= len1 && len2 >= len3)
401  {
402  mmax[0] = len2;
403  evecs[0] = vec_tmp[1] * rsqrtf (len2);
404  min_el = len3 <= mmax[min_el] ? 0 : min_el;
405  max_el = len3 > mmax[max_el] ? 0 : max_el;
406  }
407  else
408  {
409  mmax[0] = len3;
410  evecs[0] = vec_tmp[2] * rsqrtf (len3);
411  min_el = len3 <= mmax[min_el] ? 0 : min_el;
412  max_el = len3 > mmax[max_el] ? 0 : max_el;
413  }
414 
415  unsigned mid_el = 3 - min_el - max_el;
416  evecs[min_el] = normalized( cross( evecs[(min_el+1) % 3], evecs[(min_el+2) % 3] ) );
417  evecs[mid_el] = normalized( cross( evecs[(mid_el+1) % 3], evecs[(mid_el+2) % 3] ) );
418  }
419  // Rescale back to the original size.
420  evals *= scale;
421  }
422  private:
423  volatile float* mat_pkg;
424 
425  __device__ __forceinline__ float m00() const { return mat_pkg[0]; }
426  __device__ __forceinline__ float m01() const { return mat_pkg[1]; }
427  __device__ __forceinline__ float m02() const { return mat_pkg[2]; }
428  __device__ __forceinline__ float m10() const { return mat_pkg[1]; }
429  __device__ __forceinline__ float m11() const { return mat_pkg[3]; }
430  __device__ __forceinline__ float m12() const { return mat_pkg[4]; }
431  __device__ __forceinline__ float m20() const { return mat_pkg[2]; }
432  __device__ __forceinline__ float m21() const { return mat_pkg[4]; }
433  __device__ __forceinline__ float m22() const { return mat_pkg[5]; }
434 
435  __device__ __forceinline__ float3 row0() const { return make_float3( m00(), m01(), m02() ); }
436  __device__ __forceinline__ float3 row1() const { return make_float3( m10(), m11(), m12() ); }
437  __device__ __forceinline__ float3 row2() const { return make_float3( m20(), m21(), m22() ); }
438 
439  __device__ __forceinline__ static bool isMuchSmallerThan (float x, float y)
440  {
441  // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
443  return x * x <= prec_sqr * y * y;
444  }
445 
446  };
447  }
448 }
449 
450 #endif /* PCL_GPU_FEATURES_EIGEN_HPP_ */
__device__ __host__ __forceinline__ void swap(T &a, T &b)
Definition: utils.hpp:49
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:118
MiniMat< 4 > Mat43
Definition: eigen.hpp:173
__device__ __host__ __forceinline__ const float3 & operator[](int i) const
Definition: eigen.hpp:170
__device__ __host__ __forceinline__ float3 cross(const float3 &v1, const float3 &v2)
Definition: utils.hpp:124
__device__ __forceinline__ void compute(Mat33 &tmp, Mat33 &vec_tmp, Mat33 &evecs, float3 &evals)
Definition: eigen.hpp:209
__device__ __forceinline__ float dot(const float3 &v1, const float3 &v2)
Definition: utils.hpp:76
__device__ static __forceinline__ type epsilon()
Definition: limits.hpp:49
static __forceinline__ __device__ float3 unitOrthogonal(const float3 &src)
Definition: eigen.hpp:176
__device__ __forceinline__ void computeRoots2(const float &b, const float &c, float3 &roots)
Definition: eigen.hpp:101
__device__ __forceinline__ Eigen33(volatile float *mat_pkg_arg)
Definition: eigen.hpp:208
MiniMat< 3 > Mat33
Definition: eigen.hpp:172
__device__ __host__ __forceinline__ float3 & operator[](int i)
Definition: eigen.hpp:169
__device__ __forceinline__ void computeRoots3(float c0, float c1, float c2, float3 &roots)
Definition: eigen.hpp:114