41#include <Eigen/Geometry>
44#define AUX_PI_FLOAT 3.14159265358979323846f
45#define AUX_HALF_PI 1.57079632679489661923f
46#define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f)
54 template<
typename T>
bool
57 if ( a.first == b.first )
58 return a.second < b.second;
60 return a.first < b.first;
63 template<
typename T> T
69 template<
typename T> T
81 template<
typename T>
void
94 template<
typename T>
void
97 if ( p[0] <
bbox[0] )
bbox[0] = p[0];
98 else if ( p[0] >
bbox[1] )
bbox[1] = p[0];
100 if ( p[1] <
bbox[2] )
bbox[2] = p[1];
101 else if ( p[1] >
bbox[3] )
bbox[3] = p[1];
103 if ( p[2] <
bbox[4] )
bbox[4] = p[2];
104 else if ( p[2] >
bbox[5] )
bbox[5] = p[2];
108 template <
typename T>
void
111 v[0] = v[1] = v[2] = value;
115 template <
typename T>
void
124 template <
typename T>
void
133 template <
typename T>
void
142 template <
typename T>
bool
145 return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
149 template <
typename T>
void
158 template <
typename T>
void
159 sum3 (
const T a[3],
const T b[3], T
c[3])
167 template <
typename T>
void
168 diff3 (
const T a[3],
const T b[3], T
c[3])
175 template <
typename T>
void
184 template <
typename T> T
187 return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
191 template <
typename T> T
194 T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
199 template <
typename T> T
206 template <
typename T> T
207 dot3 (
const T a[3],
const T b[3])
209 return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
213 template <
typename T> T
214 dot3 (T x, T y, T z, T u, T v, T w)
216 return (x*u + y*v + z*w);
220 template <
typename T>
void
229 template <
typename T>
void
238 template <
typename T>
void
248 template <
typename T> T
251 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
255 template <
typename T>
void
265 template <
typename T>
void
268 m[0] =
m[4] =
m[8] = 1.0;
269 m[1] =
m[2] =
m[3] =
m[5] =
m[6] =
m[7] = 0.0;
273 template <
typename T>
void
276 out[0] = v[0]*
m[0] + v[1]*
m[1] + v[2]*
m[2];
277 out[1] = v[0]*
m[3] + v[1]*
m[4] + v[2]*
m[5];
278 out[2] = v[0]*
m[6] + v[1]*
m[7] + v[2]*
m[8];
285 template <
typename T>
void
286 mult3x3 (
const T x[3],
const T y[3],
const T z[3],
const T
m[3][3], T out[9])
288 out[0] = x[0]*
m[0][0] + y[0]*
m[1][0] + z[0]*
m[2][0];
289 out[1] = x[0]*
m[0][1] + y[0]*
m[1][1] + z[0]*
m[2][1];
290 out[2] = x[0]*
m[0][2] + y[0]*
m[1][2] + z[0]*
m[2][2];
292 out[3] = x[1]*
m[0][0] + y[1]*
m[1][0] + z[1]*
m[2][0];
293 out[4] = x[1]*
m[0][1] + y[1]*
m[1][1] + z[1]*
m[2][1];
294 out[5] = x[1]*
m[0][2] + y[1]*
m[1][2] + z[1]*
m[2][2];
296 out[6] = x[2]*
m[0][0] + y[2]*
m[1][0] + z[2]*
m[2][0];
297 out[7] = x[2]*
m[0][1] + y[2]*
m[1][1] + z[2]*
m[2][1];
298 out[8] = x[2]*
m[0][2] + y[2]*
m[1][2] + z[2]*
m[2][2];
303 template<
class T>
void
306 out[0] =
t[0]*p[0] +
t[1]*p[1] +
t[2]*p[2] +
t[9];
307 out[1] =
t[3]*p[0] +
t[4]*p[1] +
t[5]*p[2] +
t[10];
308 out[2] =
t[6]*p[0] +
t[7]*p[1] +
t[8]*p[2] +
t[11];
313 template<
class T>
void
316 out[0] =
t[0]*x +
t[1]*y +
t[2]*z +
t[9];
317 out[1] =
t[3]*x +
t[4]*y +
t[5]*z +
t[10];
318 out[2] =
t[6]*x +
t[7]*y +
t[8]*z +
t[11];
322 template<
class T>
void
325 out.x =
mat(0,0)*p.x +
mat(0,1)*p.y +
mat(0,2)*p.z +
mat(0,3);
326 out.y =
mat(1,0)*p.x +
mat(1,1)*p.y +
mat(1,2)*p.z +
mat(1,3);
327 out.z =
mat(2,0)*p.x +
mat(2,1)*p.y +
mat(2,2)*p.z +
mat(2,3);
332 template<
class T>
void
335 out[0] =
t[0]*p.x +
t[1]*p.y +
t[2]*p.z +
t[9];
336 out[1] =
t[3]*p.x +
t[4]*p.y +
t[5]*p.z +
t[10];
337 out[2] =
t[6]*p.x +
t[7]*p.y +
t[8]*p.z +
t[11];
344 template<
typename T>
bool
351 T
cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
365 template<
typename Scalar>
void
374 template<
typename Scalar>
void
387 template <
typename T>
void
400 template <
typename T>
void
410 template <
typename T>
void
436 template <
typename T>
void
455 if ( angle > AUX_PI_FLOAT )
457 angle = 2.0f*AUX_PI_FLOAT - angle;
Iterator class for point clouds with or without given indices.
Defines all the PCL implemented PointT point type structures.
T sqrLength3(const T v[3])
Returns the square length of v.
void expandBoundingBoxToContainPoint(T bbox[6], const T p[3])
Expands the bounding box 'bbox' such that it contains the point 'p'.
void array12ToMatrix4x4(const Scalar src[12], Eigen::Matrix< Scalar, 4, 4 > &dst)
T distance3(const T a[3], const T b[3])
Returns the Euclidean distance between a and b.
void axisAngleToRotationMatrix(const T axis_angle[3], T rotation_matrix[9])
brief Computes a rotation matrix from the provided input vector 'axis_angle'.
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
T sqrDistance3(const T a[3], const T b[3])
Returns the squared Euclidean distance between a and b.
bool equal3(const T a[3], const T b[3])
a = b
T length3(const T v[3])
Returns the length of v.
void mult3(T *v, T scalar)
v = scalar*v.
void matrix4x4ToArray12(const Eigen::Matrix< Scalar, 4, 4 > &src, Scalar dst[12])
void add3(T a[3], const T b[3])
a += b
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...
void normalize3(T v[3])
Normalize v.
T clamp(T value, T min, T max)
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 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 diff3(const T a[3], const T b[3], T c[3])
c = a - b
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
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'.
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 copy3(const T src[3], T dst[3])
dst = src
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
void cross3(const T v1[3], const T v2[3], T out[3])
void expandBoundingBox(T dst[6], const T src[6])
Expands the destination bounding box 'dst' such that it contains 'src'.
void identity3x3(T m[9])
Sets 'm' to the 3x3 identity matrix.
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 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'...
A point structure representing Euclidean xyz coordinates.