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;
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)
This file defines compatibility wrappers for low level I/O functions.
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.
Defines all the PCL implemented PointT point type structures.
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
bool equal3(const 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...