38 #ifndef PCL_RECOGNITION_RANSAC_BASED_AUX_H_
39 #define PCL_RECOGNITION_RANSAC_BASED_AUX_H_
43 #include <pcl/common/eigen.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>
bool
147 return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
151 template <
typename T>
void
160 template <
typename T>
void
161 sum3 (
const T a[3],
const T b[3], T c[3])
169 template <
typename T>
void
170 diff3 (
const T a[3],
const T b[3], T c[3])
177 template <
typename T>
void
178 cross3 (
const T v1[3],
const T v2[3], T out[3])
180 out[0] = v1[1]*v2[2] - v1[2]*v2[1];
181 out[1] = v1[2]*v2[0] - v1[0]*v2[2];
182 out[2] = v1[0]*v2[1] - v1[1]*v2[0];
186 template <
typename T> T
189 return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
193 template <
typename T> T
196 T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
201 template <
typename T> T
208 template <
typename T> T
209 dot3 (
const T a[3],
const T b[3])
211 return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
215 template <
typename T> T
216 dot3 (T x, T y, T z, T u, T v, T w)
218 return (x*u + y*v + z*w);
222 template <
typename T>
void
231 template <
typename T>
void
232 mult3 (
const T* v, T scalar, T* out)
234 out[0] = v[0]*scalar;
235 out[1] = v[1]*scalar;
236 out[2] = v[2]*scalar;
240 template <
typename T>
void
250 template <
typename T> T
253 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
257 template <
typename T>
void
262 T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]};
267 template <
typename T>
void
270 m[0] = m[4] = m[8] = 1.0;
271 m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0;
275 template <
typename T>
void
278 out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2];
279 out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5];
280 out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8];
287 template <
typename T>
void
288 mult3x3 (
const T x[3],
const T y[3],
const T z[3],
const T m[3][3], T out[9])
290 out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0];
291 out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1];
292 out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2];
294 out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0];
295 out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1];
296 out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2];
298 out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0];
299 out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1];
300 out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2];
305 template<
class T>
void
308 out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9];
309 out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10];
310 out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11];
315 template<
class T>
void
318 out[0] = t[0]*x + t[1]*y + t[2]*z + t[9];
319 out[1] = t[3]*x + t[4]*y + t[5]*z + t[10];
320 out[2] = t[6]*x + t[7]*y + t[8]*z + t[11];
324 template<
class T>
void
327 out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3);
328 out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3);
329 out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3);
334 template<
class T>
void
337 out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9];
338 out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10];
339 out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11];
346 template<
typename T>
bool
353 T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
360 if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle )
367 template<
typename Scalar>
void
370 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9];
371 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10];
372 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11];
373 dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0;
376 template<
typename Scalar>
void
379 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3);
380 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3);
381 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3);
389 template <
typename T>
void
392 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
393 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
394 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
402 template <
typename T>
void
405 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
406 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
407 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
412 template <
typename T>
void
425 T normalized_axis_angle[3];
426 aux::mult3 (axis_angle,
static_cast<T
> (1.0)/angle, normalized_axis_angle);
429 Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle);
430 Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis);
438 template <
typename T>
void
442 Eigen::AngleAxis<T> angle_axis;
443 Eigen::Matrix<T,3,3> rot_mat;
448 angle_axis.fromRotationMatrix (rot_mat);
451 axis[0] = angle_axis.axis () (0,0);
452 axis[1] = angle_axis.axis () (1,0);
453 axis[2] = angle_axis.axis () (2,0);
454 angle = angle_axis.angle ();
457 if ( angle > AUX_PI_FLOAT )
459 angle = 2.0f*AUX_PI_FLOAT - angle;