Point Cloud Library (PCL) 1.12.1
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 <limits>
94
95#include <pcl/gpu/utils/device/algorithm.hpp>
96#include <pcl/gpu/utils/device/vector_math.hpp>
97
98namespace pcl
99{
100 namespace device
101 {
102 __device__ __forceinline__ void computeRoots2(const float& b, const float& c, float3& roots)
103 {
104 roots.x = 0.f;
105 float d = b * b - 4.f * c;
106 if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
107 d = 0.f;
108
109 float sd = sqrtf(d);
110
111 roots.z = 0.5f * (b + sd);
112 roots.y = 0.5f * (b - sd);
113 }
114
115 __device__ __forceinline__ void computeRoots3(float c0, float c1, float c2, float3& roots)
116 {
117 if ( std::abs(c0) < std::numeric_limits<float>::epsilon())// one root is 0 -> quadratic equation
118 {
119 computeRoots2 (c2, c1, roots);
120 }
121 else
122 {
123 const float s_inv3 = 1.f/3.f;
124 const float s_sqrt3 = sqrtf(3.f);
125 // Construct the parameters used in classifying the roots of the equation
126 // and in solving the equation for the roots in closed form.
127 float c2_over_3 = c2 * s_inv3;
128 float a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
129 if (a_over_3 > 0.f)
130 a_over_3 = 0.f;
131
132 float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
133
134 float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
135 if (q > 0.f)
136 q = 0.f;
137
138 // Compute the eigenvalues by solving for the roots of the polynomial.
139 float rho = sqrtf(-a_over_3);
140 float theta = std::atan2 (sqrtf (-q), half_b)*s_inv3;
141 float cos_theta = __cosf (theta);
142 float sin_theta = __sinf (theta);
143 roots.x = c2_over_3 + 2.f * rho * cos_theta;
144 roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
145 roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
146
147 // Sort in increasing order.
148 if (roots.x >= roots.y)
149 swap(roots.x, roots.y);
150
151 if (roots.y >= roots.z)
152 {
153 swap(roots.y, roots.z);
154
155 if (roots.x >= roots.y)
156 swap (roots.x, roots.y);
157 }
158 if (roots.x <= 0) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
159 computeRoots2 (c2, c1, roots);
160 }
161 }
162
163 struct Eigen33
164 {
165 public:
166 template<int Rows>
167 struct MiniMat
168 {
169 float3 data[Rows];
170 __device__ __host__ __forceinline__ float3& operator[](int i) { return data[i]; }
171 __device__ __host__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
172 };
175
176
177 static __forceinline__ __device__ float3 unitOrthogonal (const float3& src)
178 {
179 float3 perp;
180 /* Let us compute the crossed product of *this with a vector
181 * that is not too close to being colinear to *this.
182 */
183
184 /* unless the x and y coords are both close to zero, we can
185 * simply take ( -y, x, 0 ) and normalize it.
186 */
187 if(!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
188 {
189 float invnm = rsqrtf(src.x*src.x + src.y*src.y);
190 perp.x = -src.y * invnm;
191 perp.y = src.x * invnm;
192 perp.z = 0.0f;
193 }
194 /* if both x and y are close to zero, then the vector is close
195 * to the z-axis, so it's far from colinear to the x-axis for instance.
196 * So we take the crossed product with (1,0,0) and normalize it.
197 */
198 else
199 {
200 float invnm = rsqrtf(src.z * src.z + src.y * src.y);
201 perp.x = 0.0f;
202 perp.y = -src.z * invnm;
203 perp.z = src.y * invnm;
204 }
205
206 return perp;
207 }
208
209 __device__ __forceinline__ Eigen33(volatile float* mat_pkg_arg) : mat_pkg(mat_pkg_arg) {}
210 __device__ __forceinline__ void compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals)
211 {
212 // Scale the matrix so its entries are in [-1,1]. The scaling is applied
213 // only when at least one matrix entry has magnitude larger than 1.
214
215 float max01 = fmaxf( std::abs(mat_pkg[0]), std::abs(mat_pkg[1]) );
216 float max23 = fmaxf( std::abs(mat_pkg[2]), std::abs(mat_pkg[3]) );
217 float max45 = fmaxf( std::abs(mat_pkg[4]), std::abs(mat_pkg[5]) );
218 float m0123 = fmaxf( max01, max23);
219 float scale = fmaxf( max45, m0123);
220
221 if (scale <= std::numeric_limits<float>::min())
222 scale = 1.f;
223
224 mat_pkg[0] /= scale;
225 mat_pkg[1] /= scale;
226 mat_pkg[2] /= scale;
227 mat_pkg[3] /= scale;
228 mat_pkg[4] /= scale;
229 mat_pkg[5] /= scale;
230
231 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
232 // eigenvalues are the roots to this equation, all guaranteed to be
233 // real-valued, because the matrix is symmetric.
234 float c0 = m00() * m11() * m22()
235 + 2.f * m01() * m02() * m12()
236 - m00() * m12() * m12()
237 - m11() * m02() * m02()
238 - m22() * m01() * m01();
239 float c1 = m00() * m11() -
240 m01() * m01() +
241 m00() * m22() -
242 m02() * m02() +
243 m11() * m22() -
244 m12() * m12();
245 float c2 = m00() + m11() + m22();
246
247 computeRoots3(c0, c1, c2, evals);
248
249 if(evals.z - evals.x <= std::numeric_limits<float>::epsilon())
250 {
251 evecs[0] = make_float3(1.f, 0.f, 0.f);
252 evecs[1] = make_float3(0.f, 1.f, 0.f);
253 evecs[2] = make_float3(0.f, 0.f, 1.f);
254 }
255 else if (evals.y - evals.x <= std::numeric_limits<float>::epsilon() )
256 {
257 // first and second equal
258 tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
259 tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
260
261 vec_tmp[0] = cross(tmp[0], tmp[1]);
262 vec_tmp[1] = cross(tmp[0], tmp[2]);
263 vec_tmp[2] = cross(tmp[1], tmp[2]);
264
265 float len1 = dot (vec_tmp[0], vec_tmp[0]);
266 float len2 = dot (vec_tmp[1], vec_tmp[1]);
267 float len3 = dot (vec_tmp[2], vec_tmp[2]);
268
269 if (len1 >= len2 && len1 >= len3)
270 {
271 evecs[2] = vec_tmp[0] * rsqrtf (len1);
272 }
273 else if (len2 >= len1 && len2 >= len3)
274 {
275 evecs[2] = vec_tmp[1] * rsqrtf (len2);
276 }
277 else
278 {
279 evecs[2] = vec_tmp[2] * rsqrtf (len3);
280 }
281
282 evecs[1] = unitOrthogonal(evecs[2]);
283 evecs[0] = cross(evecs[1], evecs[2]);
284 }
285 else if (evals.z - evals.y <= std::numeric_limits<float>::epsilon() )
286 {
287 // second and third equal
288 tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
289 tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
290
291 vec_tmp[0] = cross(tmp[0], tmp[1]);
292 vec_tmp[1] = cross(tmp[0], tmp[2]);
293 vec_tmp[2] = cross(tmp[1], tmp[2]);
294
295 float len1 = dot(vec_tmp[0], vec_tmp[0]);
296 float len2 = dot(vec_tmp[1], vec_tmp[1]);
297 float len3 = dot(vec_tmp[2], vec_tmp[2]);
298
299 if (len1 >= len2 && len1 >= len3)
300 {
301 evecs[0] = vec_tmp[0] * rsqrtf(len1);
302 }
303 else if (len2 >= len1 && len2 >= len3)
304 {
305 evecs[0] = vec_tmp[1] * rsqrtf(len2);
306 }
307 else
308 {
309 evecs[0] = vec_tmp[2] * rsqrtf(len3);
310 }
311
312 evecs[1] = unitOrthogonal( evecs[0] );
313 evecs[2] = cross(evecs[0], evecs[1]);
314 }
315 else
316 {
317
318 tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
319 tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
320
321 vec_tmp[0] = cross(tmp[0], tmp[1]);
322 vec_tmp[1] = cross(tmp[0], tmp[2]);
323 vec_tmp[2] = cross(tmp[1], tmp[2]);
324
325 float len1 = dot(vec_tmp[0], vec_tmp[0]);
326 float len2 = dot(vec_tmp[1], vec_tmp[1]);
327 float len3 = dot(vec_tmp[2], vec_tmp[2]);
328
329 float mmax[3];
330
331 unsigned int min_el = 2;
332 unsigned int max_el = 2;
333 if (len1 >= len2 && len1 >= len3)
334 {
335 mmax[2] = len1;
336 evecs[2] = vec_tmp[0] * rsqrtf (len1);
337 }
338 else if (len2 >= len1 && len2 >= len3)
339 {
340 mmax[2] = len2;
341 evecs[2] = vec_tmp[1] * rsqrtf (len2);
342 }
343 else
344 {
345 mmax[2] = len3;
346 evecs[2] = vec_tmp[2] * rsqrtf (len3);
347 }
348
349 tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
350 tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
351
352 vec_tmp[0] = cross(tmp[0], tmp[1]);
353 vec_tmp[1] = cross(tmp[0], tmp[2]);
354 vec_tmp[2] = cross(tmp[1], tmp[2]);
355
356 len1 = dot(vec_tmp[0], vec_tmp[0]);
357 len2 = dot(vec_tmp[1], vec_tmp[1]);
358 len3 = dot(vec_tmp[2], vec_tmp[2]);
359
360 if (len1 >= len2 && len1 >= len3)
361 {
362 mmax[1] = len1;
363 evecs[1] = vec_tmp[0] * rsqrtf (len1);
364 min_el = len1 <= mmax[min_el] ? 1 : min_el;
365 max_el = len1 > mmax[max_el] ? 1 : max_el;
366 }
367 else if (len2 >= len1 && len2 >= len3)
368 {
369 mmax[1] = len2;
370 evecs[1] = vec_tmp[1] * rsqrtf (len2);
371 min_el = len2 <= mmax[min_el] ? 1 : min_el;
372 max_el = len2 > mmax[max_el] ? 1 : max_el;
373 }
374 else
375 {
376 mmax[1] = len3;
377 evecs[1] = vec_tmp[2] * rsqrtf (len3);
378 min_el = len3 <= mmax[min_el] ? 1 : min_el;
379 max_el = len3 > mmax[max_el] ? 1 : max_el;
380 }
381
382 tmp[0] = row0(); tmp[1] = row1(); tmp[2] = row2();
383 tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
384
385 vec_tmp[0] = cross(tmp[0], tmp[1]);
386 vec_tmp[1] = cross(tmp[0], tmp[2]);
387 vec_tmp[2] = cross(tmp[1], tmp[2]);
388
389 len1 = dot (vec_tmp[0], vec_tmp[0]);
390 len2 = dot (vec_tmp[1], vec_tmp[1]);
391 len3 = dot (vec_tmp[2], vec_tmp[2]);
392
393
394 if (len1 >= len2 && len1 >= len3)
395 {
396 mmax[0] = len1;
397 evecs[0] = vec_tmp[0] * rsqrtf (len1);
398 min_el = len3 <= mmax[min_el] ? 0 : min_el;
399 max_el = len3 > mmax[max_el] ? 0 : max_el;
400 }
401 else if (len2 >= len1 && len2 >= len3)
402 {
403 mmax[0] = len2;
404 evecs[0] = vec_tmp[1] * rsqrtf (len2);
405 min_el = len3 <= mmax[min_el] ? 0 : min_el;
406 max_el = len3 > mmax[max_el] ? 0 : max_el;
407 }
408 else
409 {
410 mmax[0] = len3;
411 evecs[0] = vec_tmp[2] * rsqrtf (len3);
412 min_el = len3 <= mmax[min_el] ? 0 : min_el;
413 max_el = len3 > mmax[max_el] ? 0 : max_el;
414 }
415
416 unsigned mid_el = 3 - min_el - max_el;
417 evecs[min_el] = normalized( cross( evecs[(min_el+1) % 3], evecs[(min_el+2) % 3] ) );
418 evecs[mid_el] = normalized( cross( evecs[(mid_el+1) % 3], evecs[(mid_el+2) % 3] ) );
419 }
420 // Rescale back to the original size.
421 evals *= scale;
422 }
423 private:
424 volatile float* mat_pkg;
425
426 __device__ __forceinline__ float m00() const { return mat_pkg[0]; }
427 __device__ __forceinline__ float m01() const { return mat_pkg[1]; }
428 __device__ __forceinline__ float m02() const { return mat_pkg[2]; }
429 __device__ __forceinline__ float m10() const { return mat_pkg[1]; }
430 __device__ __forceinline__ float m11() const { return mat_pkg[3]; }
431 __device__ __forceinline__ float m12() const { return mat_pkg[4]; }
432 __device__ __forceinline__ float m20() const { return mat_pkg[2]; }
433 __device__ __forceinline__ float m21() const { return mat_pkg[4]; }
434 __device__ __forceinline__ float m22() const { return mat_pkg[5]; }
435
436 __device__ __forceinline__ float3 row0() const { return make_float3( m00(), m01(), m02() ); }
437 __device__ __forceinline__ float3 row1() const { return make_float3( m10(), m11(), m12() ); }
438 __device__ __forceinline__ float3 row2() const { return make_float3( m20(), m21(), m22() ); }
439
440 __device__ __forceinline__ static bool isMuchSmallerThan (float x, float y)
441 {
442 // copied from <eigen>/include/Eigen/src/Core/NumTraits.h
443 constexpr float prec_sqr = std::numeric_limits<float>::epsilon() * std::numeric_limits<float>::epsilon();
444 return x * x <= prec_sqr * y * y;
445 }
446
447 };
448 }
449}
450
451#endif /* PCL_GPU_FEATURES_EIGEN_HPP_ */
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
__device__ __forceinline__ float dot(const float3 &v1, const float3 &v2)
Definition: utils.hpp:59
__device__ __forceinline__ void computeRoots3(float c0, float c1, float c2, float3 &roots)
Definition: eigen.hpp:115
__device__ __forceinline__ void computeRoots2(const float &b, const float &c, float3 &roots)
Definition: eigen.hpp:102
__device__ __host__ __forceinline__ void swap(T &a, T &b)
Definition: utils.hpp:53
__device__ __host__ __forceinline__ float3 cross(const float3 &v1, const float3 &v2)
Definition: utils.hpp:107
__device__ __host__ __forceinline__ float3 & operator[](int i)
Definition: eigen.hpp:170
__device__ __host__ __forceinline__ const float3 & operator[](int i) const
Definition: eigen.hpp:171
__device__ __forceinline__ void compute(Mat33 &tmp, Mat33 &vec_tmp, Mat33 &evecs, float3 &evals)
Definition: eigen.hpp:210
static __forceinline__ __device__ float3 unitOrthogonal(const float3 &src)
Definition: eigen.hpp:177
__device__ __forceinline__ Eigen33(volatile float *mat_pkg_arg)
Definition: eigen.hpp:209