42 #include <pcl/common/eigen.h>
45 #define AUX_PI_FLOAT 3.14159265358979323846f
46 #define AUX_HALF_PI 1.57079632679489661923f
47 #define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f)
55 template<
typename T>
bool
58 if ( a.first == b.first )
59 return a.second < b.second;
61 return a.first < b.first;
64 template<
typename T> T
70 template<
typename T> T
82 template<
typename T>
void
85 if ( src[0] < dst[0] ) dst[0] = src[0];
86 if ( src[2] < dst[2] ) dst[2] = src[2];
87 if ( src[4] < dst[4] ) dst[4] = src[4];
89 if ( src[1] > dst[1] ) dst[1] = src[1];
90 if ( src[3] > dst[3] ) dst[3] = src[3];
91 if ( src[5] > dst[5] ) dst[5] = src[5];
95 template<
typename T>
void
98 if ( p[0] < bbox[0] ) bbox[0] = p[0];
99 else if ( p[0] > bbox[1] ) bbox[1] = p[0];
101 if ( p[1] < bbox[2] ) bbox[2] = p[1];
102 else if ( p[1] > bbox[3] ) bbox[3] = p[1];
104 if ( p[2] < bbox[4] ) bbox[4] = p[2];
105 else if ( p[2] > bbox[5] ) bbox[5] = p[2];
109 template <
typename T>
void
112 v[0] = v[1] = v[2] = value;
116 template <
typename T>
void
125 template <
typename T>
void
134 template <
typename T>
void
143 template <
typename T>
bool
146 return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
150 template <
typename T>
void
159 template <
typename T>
void
160 sum3 (
const T a[3],
const T b[3], T c[3])
168 template <
typename T>
void
169 diff3 (
const T a[3],
const T b[3], T c[3])
176 template <
typename T>
void
177 cross3 (
const T v1[3],
const T v2[3], T out[3])
179 out[0] = v1[1]*v2[2] - v1[2]*v2[1];
180 out[1] = v1[2]*v2[0] - v1[0]*v2[2];
181 out[2] = v1[0]*v2[1] - v1[1]*v2[0];
185 template <
typename T> T
188 return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
192 template <
typename T> T
195 T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
200 template <
typename T> T
207 template <
typename T> T
208 dot3 (
const T a[3],
const T b[3])
210 return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
214 template <
typename T> T
215 dot3 (T x, T y, T z, T u, T v, T w)
217 return (x*u + y*v + z*w);
221 template <
typename T>
void
230 template <
typename T>
void
231 mult3 (
const T* v, T scalar, T* out)
233 out[0] = v[0]*scalar;
234 out[1] = v[1]*scalar;
235 out[2] = v[2]*scalar;
239 template <
typename T>
void
249 template <
typename T> T
252 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
256 template <
typename T>
void
261 T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]};
266 template <
typename T>
void
269 m[0] = m[4] = m[8] = 1.0;
270 m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0;
274 template <
typename T>
void
277 out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2];
278 out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5];
279 out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8];
286 template <
typename T>
void
287 mult3x3 (
const T x[3],
const T y[3],
const T z[3],
const T m[3][3], T out[9])
289 out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0];
290 out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1];
291 out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2];
293 out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0];
294 out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1];
295 out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2];
297 out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0];
298 out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1];
299 out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2];
304 template<
class T>
void
307 out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9];
308 out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10];
309 out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11];
314 template<
class T>
void
317 out[0] = t[0]*x + t[1]*y + t[2]*z + t[9];
318 out[1] = t[3]*x + t[4]*y + t[5]*z + t[10];
319 out[2] = t[6]*x + t[7]*y + t[8]*z + t[11];
323 template<
class T>
void
326 out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3);
327 out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3);
328 out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3);
333 template<
class T>
void
336 out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9];
337 out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10];
338 out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11];
345 template<
typename T>
bool
352 T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
359 if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle )
366 template<
typename Scalar>
void
369 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9];
370 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10];
371 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11];
372 dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0;
375 template<
typename Scalar>
void
378 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3);
379 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3);
380 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3);
388 template <
typename T>
void
391 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
392 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
393 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
401 template <
typename T>
void
404 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
405 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
406 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
411 template <
typename T>
void
424 T normalized_axis_angle[3];
425 aux::mult3 (axis_angle,
static_cast<T
> (1.0)/angle, normalized_axis_angle);
428 Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle);
429 Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis);
437 template <
typename T>
void
441 Eigen::AngleAxis<T> angle_axis;
442 Eigen::Matrix<T,3,3> rot_mat;
447 angle_axis.fromRotationMatrix (rot_mat);
450 axis[0] = angle_axis.axis () (0,0);
451 axis[1] = angle_axis.axis () (1,0);
452 axis[2] = angle_axis.axis () (2,0);
453 angle = angle_axis.angle ();
456 if ( angle > AUX_PI_FLOAT )
458 angle = 2.0f*AUX_PI_FLOAT - angle;