38 #ifndef PCL_RECOGNITION_RANSAC_BASED_AUX_H_
39 #define PCL_RECOGNITION_RANSAC_BASED_AUX_H_
43 #include <pcl/common/eigen.h>
44 #include <pcl/point_types.h>
46 #define AUX_PI_FLOAT 3.14159265358979323846f
47 #define AUX_HALF_PI 1.57079632679489661923f
48 #define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f)
56 template<
typename T>
bool
59 if ( a.first == b.first )
60 return static_cast<bool> (a.second < b.second);
62 return static_cast<bool> (a.first < b.first);
65 template<
typename T> T
71 template<
typename T> T
76 else if ( value > max )
83 template<
typename T>
void
86 if ( src[0] < dst[0] ) dst[0] = src[0];
87 if ( src[2] < dst[2] ) dst[2] = src[2];
88 if ( src[4] < dst[4] ) dst[4] = src[4];
90 if ( src[1] > dst[1] ) dst[1] = src[1];
91 if ( src[3] > dst[3] ) dst[3] = src[3];
92 if ( src[5] > dst[5] ) dst[5] = src[5];
96 template<
typename T>
void
99 if ( p[0] < bbox[0] ) bbox[0] = p[0];
100 else if ( p[0] > bbox[1] ) bbox[1] = p[0];
102 if ( p[1] < bbox[2] ) bbox[2] = p[1];
103 else if ( p[1] > bbox[3] ) bbox[3] = p[1];
105 if ( p[2] < bbox[4] ) bbox[4] = p[2];
106 else if ( p[2] > bbox[5] ) bbox[5] = p[2];
110 template <
typename T>
void
113 v[0] = v[1] = v[2] = value;
117 template <
typename T>
void
126 template <
typename T>
void
135 template <
typename T>
void
144 template <
typename T>
void
153 template <
typename T>
void
154 sum3 (
const T a[3],
const T b[3], T c[3])
162 template <
typename T>
void
163 diff3 (
const T a[3],
const T b[3], T c[3])
170 template <
typename T>
void
171 cross3 (
const T v1[3],
const T v2[3], T out[3])
173 out[0] = v1[1]*v2[2] - v1[2]*v2[1];
174 out[1] = v1[2]*v2[0] - v1[0]*v2[2];
175 out[2] = v1[0]*v2[1] - v1[1]*v2[0];
179 template <
typename T> T
182 return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
186 template <
typename T> T
189 T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
194 template <
typename T> T
201 template <
typename T> T
202 dot3 (
const T a[3],
const T b[3])
204 return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
208 template <
typename T> T
209 dot3 (T x, T y, T z, T u, T v, T w)
211 return (x*u + y*v + z*w);
215 template <
typename T>
void
224 template <
typename T>
void
225 mult3 (
const T* v, T scalar, T* out)
227 out[0] = v[0]*scalar;
228 out[1] = v[1]*scalar;
229 out[2] = v[2]*scalar;
233 template <
typename T>
void
243 template <
typename T> T
246 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
250 template <
typename T>
void
255 T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]};
260 template <
typename T>
void
263 m[0] = m[4] = m[8] = 1.0;
264 m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0;
268 template <
typename T>
void
271 out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2];
272 out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5];
273 out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8];
280 template <
typename T>
void
281 mult3x3 (
const T x[3],
const T y[3],
const T z[3],
const T m[3][3], T out[9])
283 out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0];
284 out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1];
285 out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2];
287 out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0];
288 out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1];
289 out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2];
291 out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0];
292 out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1];
293 out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2];
298 template<
class T>
void
301 out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9];
302 out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10];
303 out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11];
308 template<
class T>
void
311 out[0] = t[0]*x + t[1]*y + t[2]*z + t[9];
312 out[1] = t[3]*x + t[4]*y + t[5]*z + t[10];
313 out[2] = t[6]*x + t[7]*y + t[8]*z + t[11];
317 template<
class T>
void
320 out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3);
321 out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3);
322 out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3);
327 template<
class T>
void
330 out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9];
331 out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10];
332 out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11];
339 template<
typename T>
bool
346 T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
353 if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle )
360 template<
typename Scalar>
void
363 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9];
364 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10];
365 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11];
366 dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0;
369 template<
typename Scalar>
void
372 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3);
373 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3);
374 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3);
382 template <
typename T>
void
385 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
386 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
387 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
395 template <
typename T>
void
398 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
399 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
400 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
405 template <
typename T>
void
418 T normalized_axis_angle[3];
419 aux::mult3 (axis_angle, static_cast<T> (1.0)/angle, normalized_axis_angle);
422 Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle);
423 Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis);
431 template <
typename T>
void
435 Eigen::AngleAxis<T> angle_axis;
436 Eigen::Matrix<T,3,3> rot_mat;
441 angle_axis.fromRotationMatrix (rot_mat);
444 axis[0] = angle_axis.axis () (0,0);
445 axis[1] = angle_axis.axis () (1,0);
446 axis[2] = angle_axis.axis () (2,0);
447 angle = angle_axis.angle ();
450 if ( angle > AUX_PI_FLOAT )
452 angle = 2.0f*AUX_PI_FLOAT - angle;