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;
void expandBoundingBox(T dst[6], const T src[6])
Expands the destination bounding box 'dst' such that it contains 'src'.
T sqrDistance3(const T a[3], const T b[3])
Returns the squared Euclidean distance between a and b.
bool pointsAreCoplanar(const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle)
Returns true if the points 'p1' and 'p2' are co-planar and false otherwise.
void mult3(T *v, T scalar)
v = scalar*v.
bool compareOrderedPairs(const std::pair< T, T > &a, const std::pair< T, T > &b)
void mult3x3(const T m[9], const T v[3], T out[3])
out = mat*v.
void toEigenMatrix3x3RowMajor(const T src[9], Eigen::Matrix< T, 3, 3 > &dst)
The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
void projectOnPlane3(const T x[3], const T planeNormal[3], T out[3])
Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'...
void expandBoundingBoxToContainPoint(T bbox[6], const T p[3])
Expands the bounding box 'bbox' such that it contains the point 'p'.
void cross3(const T v1[3], const T v2[3], T out[3])
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
A point structure representing Euclidean xyz coordinates.
T clamp(T value, T min, T max)
T sqrLength3(const T v[3])
Returns the square length of v.
void rotationMatrixToAxisAngle(const T rotation_matrix[9], T axis[3], T &angle)
brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis'...
T distance3(const T a[3], const T b[3])
Returns the Euclidean distance between a and b.
void identity3x3(T m[9])
Sets 'm' to the 3x3 identity matrix.
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
void axisAngleToRotationMatrix(const T axis_angle[3], T rotation_matrix[9])
brief Computes a rotation matrix from the provided input vector 'axis_angle'.
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
void normalize3(T v[3])
Normalize v.
void matrix4x4ToArray12(const Eigen::Matrix< Scalar, 4, 4 > &src, Scalar dst[12])
void array12ToMatrix4x4(const Scalar src[12], Eigen::Matrix< Scalar, 4, 4 > &dst)
void copy3(const T src[3], T dst[3])
dst = src
T length3(const T v[3])
Returns the length of v.
void diff3(const T a[3], const T b[3], T c[3])
c = a - b
void add3(T a[3], const T b[3])
a += b
void eigenMatrix3x3ToArray9RowMajor(const Eigen::Matrix< T, 3, 3 > &src, T dst[9])
The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
void transform(const T t[12], const T p[3], T out[3])
The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a transla...