Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_RECOGNITION_RANSAC_BASED_AUX_H_ 00039 #define PCL_RECOGNITION_RANSAC_BASED_AUX_H_ 00040 00041 #include <cmath> 00042 #include <cstdlib> 00043 #include <pcl/common/eigen.h> 00044 #include <pcl/point_types.h> 00045 00046 #define AUX_PI_FLOAT 3.14159265358979323846f 00047 #define AUX_HALF_PI 1.57079632679489661923f 00048 #define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f) 00049 00050 namespace pcl 00051 { 00052 namespace recognition 00053 { 00054 namespace aux 00055 { 00056 template<typename T> bool 00057 compareOrderedPairs (const std::pair<T,T>& a, const std::pair<T,T>& b) 00058 { 00059 if ( a.first == b.first ) 00060 return static_cast<bool> (a.second < b.second); 00061 00062 return static_cast<bool> (a.first < b.first); 00063 } 00064 00065 template<typename T> T 00066 sqr (T a) 00067 { 00068 return (a*a); 00069 } 00070 00071 template<typename T> T 00072 clamp (T value, T min, T max) 00073 { 00074 if ( value < min ) 00075 return min; 00076 else if ( value > max ) 00077 return max; 00078 00079 return value; 00080 } 00081 00082 /** \brief Expands the destination bounding box 'dst' such that it contains 'src'. */ 00083 template<typename T> void 00084 expandBoundingBox (T dst[6], const T src[6]) 00085 { 00086 if ( src[0] < dst[0] ) dst[0] = src[0]; 00087 if ( src[2] < dst[2] ) dst[2] = src[2]; 00088 if ( src[4] < dst[4] ) dst[4] = src[4]; 00089 00090 if ( src[1] > dst[1] ) dst[1] = src[1]; 00091 if ( src[3] > dst[3] ) dst[3] = src[3]; 00092 if ( src[5] > dst[5] ) dst[5] = src[5]; 00093 } 00094 00095 /** \brief Expands the bounding box 'bbox' such that it contains the point 'p'. */ 00096 template<typename T> void 00097 expandBoundingBoxToContainPoint (T bbox[6], const T p[3]) 00098 { 00099 if ( p[0] < bbox[0] ) bbox[0] = p[0]; 00100 else if ( p[0] > bbox[1] ) bbox[1] = p[0]; 00101 00102 if ( p[1] < bbox[2] ) bbox[2] = p[1]; 00103 else if ( p[1] > bbox[3] ) bbox[3] = p[1]; 00104 00105 if ( p[2] < bbox[4] ) bbox[4] = p[2]; 00106 else if ( p[2] > bbox[5] ) bbox[5] = p[2]; 00107 } 00108 00109 /** \brief v[0] = v[1] = v[2] = value */ 00110 template <typename T> void 00111 set3 (T v[3], T value) 00112 { 00113 v[0] = v[1] = v[2] = value; 00114 } 00115 00116 /** \brief dst = src */ 00117 template <typename T> void 00118 copy3 (const T src[3], T dst[3]) 00119 { 00120 dst[0] = src[0]; 00121 dst[1] = src[1]; 00122 dst[2] = src[2]; 00123 } 00124 00125 /** \brief dst = src */ 00126 template <typename T> void 00127 copy3 (const T src[3], pcl::PointXYZ& dst) 00128 { 00129 dst.x = src[0]; 00130 dst.y = src[1]; 00131 dst.z = src[2]; 00132 } 00133 00134 /** \brief a = -a */ 00135 template <typename T> void 00136 flip3 (T a[3]) 00137 { 00138 a[0] = -a[0]; 00139 a[1] = -a[1]; 00140 a[2] = -a[2]; 00141 } 00142 00143 /** \brief a += b */ 00144 template <typename T> void 00145 add3 (T a[3], const T b[3]) 00146 { 00147 a[0] += b[0]; 00148 a[1] += b[1]; 00149 a[2] += b[2]; 00150 } 00151 00152 /** \brief c = a + b */ 00153 template <typename T> void 00154 sum3 (const T a[3], const T b[3], T c[3]) 00155 { 00156 c[0] = a[0] + b[0]; 00157 c[1] = a[1] + b[1]; 00158 c[2] = a[2] + b[2]; 00159 } 00160 00161 /** \brief c = a - b */ 00162 template <typename T> void 00163 diff3 (const T a[3], const T b[3], T c[3]) 00164 { 00165 c[0] = a[0] - b[0]; 00166 c[1] = a[1] - b[1]; 00167 c[2] = a[2] - b[2]; 00168 } 00169 00170 template <typename T> void 00171 cross3 (const T v1[3], const T v2[3], T out[3]) 00172 { 00173 out[0] = v1[1]*v2[2] - v1[2]*v2[1]; 00174 out[1] = v1[2]*v2[0] - v1[0]*v2[2]; 00175 out[2] = v1[0]*v2[1] - v1[1]*v2[0]; 00176 } 00177 00178 /** \brief Returns the length of v. */ 00179 template <typename T> T 00180 length3 (const T v[3]) 00181 { 00182 return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); 00183 } 00184 00185 /** \brief Returns the Euclidean distance between a and b. */ 00186 template <typename T> T 00187 distance3 (const T a[3], const T b[3]) 00188 { 00189 T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]}; 00190 return (length3 (l)); 00191 } 00192 00193 /** \brief Returns the squared Euclidean distance between a and b. */ 00194 template <typename T> T 00195 sqrDistance3 (const T a[3], const T b[3]) 00196 { 00197 return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2])); 00198 } 00199 00200 /** \brief Returns the dot product a*b */ 00201 template <typename T> T 00202 dot3 (const T a[3], const T b[3]) 00203 { 00204 return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]); 00205 } 00206 00207 /** \brief Returns the dot product (x, y, z)*(u, v, w) = x*u + y*v + z*w */ 00208 template <typename T> T 00209 dot3 (T x, T y, T z, T u, T v, T w) 00210 { 00211 return (x*u + y*v + z*w); 00212 } 00213 00214 /** \brief v = scalar*v. */ 00215 template <typename T> void 00216 mult3 (T* v, T scalar) 00217 { 00218 v[0] *= scalar; 00219 v[1] *= scalar; 00220 v[2] *= scalar; 00221 } 00222 00223 /** \brief out = scalar*v. */ 00224 template <typename T> void 00225 mult3 (const T* v, T scalar, T* out) 00226 { 00227 out[0] = v[0]*scalar; 00228 out[1] = v[1]*scalar; 00229 out[2] = v[2]*scalar; 00230 } 00231 00232 /** \brief Normalize v */ 00233 template <typename T> void 00234 normalize3 (T v[3]) 00235 { 00236 T inv_len = (static_cast<T> (1.0))/aux::length3 (v); 00237 v[0] *= inv_len; 00238 v[1] *= inv_len; 00239 v[2] *= inv_len; 00240 } 00241 00242 /** \brief Returns the square length of v. */ 00243 template <typename T> T 00244 sqrLength3 (const T v[3]) 00245 { 00246 return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); 00247 } 00248 00249 /** Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'. */ 00250 template <typename T> void 00251 projectOnPlane3 (const T x[3], const T planeNormal[3], T out[3]) 00252 { 00253 T dot = aux::dot3 (planeNormal, x); 00254 // Project 'x' on the plane normal 00255 T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]}; 00256 aux::sum3 (x, nproj, out); 00257 } 00258 00259 /** \brief Sets 'm' to the 3x3 identity matrix. */ 00260 template <typename T> void 00261 identity3x3 (T m[9]) 00262 { 00263 m[0] = m[4] = m[8] = 1.0; 00264 m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0; 00265 } 00266 00267 /** \brief out = mat*v. 'm' is an 1D array of 9 elements treated as a 3x3 matrix (row major order). */ 00268 template <typename T> void 00269 mult3x3(const T m[9], const T v[3], T out[3]) 00270 { 00271 out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2]; 00272 out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5]; 00273 out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8]; 00274 } 00275 00276 /** Let x, y, z be the columns of the matrix a = [x|y|z]. The method computes out = a*m. 00277 * Note that 'out' is a 1D array of 9 elements and the resulting matrix is stored in row 00278 * major order, i.e., the first matrix row is (out[0] out[1] out[2]), the second 00279 * (out[3] out[4] out[5]) and the third (out[6] out[7] out[8]). */ 00280 template <typename T> void 00281 mult3x3 (const T x[3], const T y[3], const T z[3], const T m[3][3], T out[9]) 00282 { 00283 out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0]; 00284 out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1]; 00285 out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2]; 00286 00287 out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0]; 00288 out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1]; 00289 out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2]; 00290 00291 out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0]; 00292 out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1]; 00293 out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2]; 00294 } 00295 00296 /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. 00297 * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ 00298 template<class T> void 00299 transform(const T t[12], const T p[3], T out[3]) 00300 { 00301 out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9]; 00302 out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10]; 00303 out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11]; 00304 } 00305 00306 /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. 00307 * First, (x, y, z) is multiplied by that matrix and then translated. The result is saved in 'out'. */ 00308 template<class T> void 00309 transform(const T t[12], T x, T y, T z, T out[3]) 00310 { 00311 out[0] = t[0]*x + t[1]*y + t[2]*z + t[9]; 00312 out[1] = t[3]*x + t[4]*y + t[5]*z + t[10]; 00313 out[2] = t[6]*x + t[7]*y + t[8]*z + t[11]; 00314 } 00315 00316 /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */ 00317 template<class T> void 00318 transform(const Eigen::Matrix<T,4,4>& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out) 00319 { 00320 out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3); 00321 out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3); 00322 out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3); 00323 } 00324 00325 /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation. 00326 * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */ 00327 template<class T> void 00328 transform(const T t[12], const pcl::PointXYZ& p, T out[3]) 00329 { 00330 out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9]; 00331 out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10]; 00332 out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11]; 00333 } 00334 00335 /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1' 00336 * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger 00337 * the value the larger the deviation between the normals can be which still leads to a positive test result. The 00338 * angle has to be in radians. */ 00339 template<typename T> bool 00340 pointsAreCoplanar (const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle) 00341 { 00342 // Compute the angle between 'n1' and 'n2' and compare it with 'max_angle' 00343 if ( std::acos (aux::clamp (aux::dot3 (n1, n2), -1.0f, 1.0f)) > max_angle ) 00344 return (false); 00345 00346 T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]}; 00347 aux::normalize3 (cl); 00348 00349 // Compute the angle between 'cl' and 'n1' 00350 T tmp_angle = std::acos (aux::clamp (aux::dot3 (n1, cl), -1.0f, 1.0f)); 00351 00352 // 'tmp_angle' should not deviate too much from 90 degrees 00353 if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle ) 00354 return (false); 00355 00356 // All tests passed => the points are coplanar 00357 return (true); 00358 } 00359 00360 template<typename Scalar> void 00361 array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix<Scalar, 4, 4>& dst) 00362 { 00363 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9]; 00364 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10]; 00365 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11]; 00366 dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0; 00367 } 00368 00369 template<typename Scalar> void 00370 matrix4x4ToArray12 (const Eigen::Matrix<Scalar, 4, 4>& src, Scalar dst[12]) 00371 { 00372 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3); 00373 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3); 00374 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3); 00375 } 00376 00377 /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. 00378 * dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); 00379 * dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); 00380 * dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); 00381 * */ 00382 template <typename T> void 00383 eigenMatrix3x3ToArray9RowMajor (const Eigen::Matrix<T,3,3>& src, T dst[9]) 00384 { 00385 dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); 00386 dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); 00387 dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); 00388 } 00389 00390 /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order. 00391 * dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; 00392 * dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; 00393 * dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; 00394 * */ 00395 template <typename T> void 00396 toEigenMatrix3x3RowMajor (const T src[9], Eigen::Matrix<T,3,3>& dst) 00397 { 00398 dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; 00399 dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; 00400 dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; 00401 } 00402 00403 /** brief Computes a rotation matrix from the provided input vector 'axis_angle'. The direction of 'axis_angle' is the rotation axis 00404 * and its magnitude is the angle of rotation about that axis. 'rotation_matrix' is the output rotation matrix saved in row major order. */ 00405 template <typename T> void 00406 axisAngleToRotationMatrix (const T axis_angle[3], T rotation_matrix[9]) 00407 { 00408 // Get the angle of rotation 00409 T angle = aux::length3 (axis_angle); 00410 if ( angle == 0.0 ) 00411 { 00412 // Undefined rotation -> set to identity 00413 aux::identity3x3 (rotation_matrix); 00414 return; 00415 } 00416 00417 // Normalize the input 00418 T normalized_axis_angle[3]; 00419 aux::mult3 (axis_angle, static_cast<T> (1.0)/angle, normalized_axis_angle); 00420 00421 // The eigen objects 00422 Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle); 00423 Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis); 00424 00425 // Save the output 00426 aux::eigenMatrix3x3ToArray9RowMajor (eigen_angle_axis.toRotationMatrix (), rotation_matrix); 00427 } 00428 00429 /** brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis' and an 'angle' 00430 * of rotation about that axis from the provided input. The output 'angle' is in the range [0, pi] and 'axis' is normalized. */ 00431 template <typename T> void 00432 rotationMatrixToAxisAngle (const T rotation_matrix[9], T axis[3], T& angle) 00433 { 00434 // The eigen objects 00435 Eigen::AngleAxis<T> angle_axis; 00436 Eigen::Matrix<T,3,3> rot_mat; 00437 // Copy the input matrix to the eigen matrix in row major order 00438 aux::toEigenMatrix3x3RowMajor (rotation_matrix, rot_mat); 00439 00440 // Do the computation 00441 angle_axis.fromRotationMatrix (rot_mat); 00442 00443 // Save the result 00444 axis[0] = angle_axis.axis () (0,0); 00445 axis[1] = angle_axis.axis () (1,0); 00446 axis[2] = angle_axis.axis () (2,0); 00447 angle = angle_axis.angle (); 00448 00449 // Make sure that 'angle' is in the range [0, pi] 00450 if ( angle > AUX_PI_FLOAT ) 00451 { 00452 angle = 2.0f*AUX_PI_FLOAT - angle; 00453 aux::flip3 (axis); 00454 } 00455 } 00456 } // namespace aux 00457 } // namespace recognition 00458 } // namespace pcl 00459 00460 #endif // AUX_H_