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) 2010-2012, Willow Garage, Inc. 00006 * Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> 00007 * Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com> 00008 * Copyright (c) 2012-, Open Perception, Inc. 00009 * 00010 * All rights reserved. 00011 * 00012 * Redistribution and use in source and binary forms, with or without 00013 * modification, are permitted provided that the following conditions 00014 * are met: 00015 * 00016 * * Redistributions of source code must retain the above copyright 00017 * notice, this list of conditions and the following disclaimer. 00018 * * Redistributions in binary form must reproduce the above 00019 * copyright notice, this list of conditions and the following 00020 * disclaimer in the documentation and/or other materials provided 00021 * with the distribution. 00022 * * Neither the name of the copyright holder(s) nor the names of its 00023 * contributors may be used to endorse or promote products derived 00024 * from this software without specific prior written permission. 00025 * 00026 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00027 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00028 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00029 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00030 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00031 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00032 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00033 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00034 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00035 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00036 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00037 * POSSIBILITY OF SUCH DAMAGE. 00038 * 00039 * $Id$ 00040 * 00041 */ 00042 #ifndef PCL_COMMON_EIGEN_H_ 00043 #define PCL_COMMON_EIGEN_H_ 00044 00045 #ifndef NOMINMAX 00046 #define NOMINMAX 00047 #endif 00048 00049 #if defined __GNUC__ 00050 # pragma GCC system_header 00051 #elif defined __SUNPRO_CC 00052 # pragma disable_warn 00053 #endif 00054 00055 #include <cmath> 00056 00057 #include <Eigen/StdVector> 00058 #include <Eigen/Core> 00059 #include <Eigen/Eigenvalues> 00060 #include <Eigen/Geometry> 00061 #include <Eigen/SVD> 00062 #include <Eigen/LU> 00063 #include <Eigen/Dense> 00064 #include <Eigen/Eigenvalues> 00065 00066 namespace pcl 00067 { 00068 /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0 00069 * \param[in] b linear parameter 00070 * \param[in] c constant parameter 00071 * \param[out] roots solutions of x^2 + b*x + c = 0 00072 */ 00073 template<typename Scalar, typename Roots> inline void 00074 computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots) 00075 { 00076 roots (0) = Scalar (0); 00077 Scalar d = Scalar (b * b - 4.0 * c); 00078 if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN! 00079 d = 0.0; 00080 00081 Scalar sd = ::std::sqrt (d); 00082 00083 roots (2) = 0.5f * (b + sd); 00084 roots (1) = 0.5f * (b - sd); 00085 } 00086 00087 /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues 00088 * \param[in] m input matrix 00089 * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues 00090 */ 00091 template<typename Matrix, typename Roots> inline void 00092 computeRoots (const Matrix& m, Roots& roots) 00093 { 00094 typedef typename Matrix::Scalar Scalar; 00095 00096 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The 00097 // eigenvalues are the roots to this equation, all guaranteed to be 00098 // real-valued, because the matrix is symmetric. 00099 Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2) 00100 + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2) 00101 - m (0, 0) * m (1, 2) * m (1, 2) 00102 - m (1, 1) * m (0, 2) * m (0, 2) 00103 - m (2, 2) * m (0, 1) * m (0, 1); 00104 Scalar c1 = m (0, 0) * m (1, 1) - 00105 m (0, 1) * m (0, 1) + 00106 m (0, 0) * m (2, 2) - 00107 m (0, 2) * m (0, 2) + 00108 m (1, 1) * m (2, 2) - 00109 m (1, 2) * m (1, 2); 00110 Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2); 00111 00112 00113 if (fabs (c0) < Eigen::NumTraits<Scalar>::epsilon ())// one root is 0 -> quadratic equation 00114 computeRoots2 (c2, c1, roots); 00115 else 00116 { 00117 const Scalar s_inv3 = Scalar (1.0 / 3.0); 00118 const Scalar s_sqrt3 = std::sqrt (Scalar (3.0)); 00119 // Construct the parameters used in classifying the roots of the equation 00120 // and in solving the equation for the roots in closed form. 00121 Scalar c2_over_3 = c2*s_inv3; 00122 Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3; 00123 if (a_over_3 > Scalar (0)) 00124 a_over_3 = Scalar (0); 00125 00126 Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1)); 00127 00128 Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3; 00129 if (q > Scalar (0)) 00130 q = Scalar (0); 00131 00132 // Compute the eigenvalues by solving for the roots of the polynomial. 00133 Scalar rho = std::sqrt (-a_over_3); 00134 Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3; 00135 Scalar cos_theta = std::cos (theta); 00136 Scalar sin_theta = std::sin (theta); 00137 roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta; 00138 roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta); 00139 roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta); 00140 00141 // Sort in increasing order. 00142 if (roots (0) >= roots (1)) 00143 std::swap (roots (0), roots (1)); 00144 if (roots (1) >= roots (2)) 00145 { 00146 std::swap (roots (1), roots (2)); 00147 if (roots (0) >= roots (1)) 00148 std::swap (roots (0), roots (1)); 00149 } 00150 00151 if (roots (0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0 00152 computeRoots2 (c2, c1, roots); 00153 } 00154 } 00155 00156 /** \brief determine the smallest eigenvalue and its corresponding eigenvector 00157 * \param[in] mat input matrix that needs to be symmetric and positive semi definite 00158 * \param[out] eigenvalue the smallest eigenvalue of the input matrix 00159 * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix 00160 * \ingroup common 00161 */ 00162 template <typename Matrix, typename Vector> inline void 00163 eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) 00164 { 00165 // if diagonal matrix, the eigenvalues are the diagonal elements 00166 // and the eigenvectors are not unique, thus set to Identity 00167 if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ()) 00168 { 00169 if (mat.coeff (0) < mat.coeff (2)) 00170 { 00171 eigenvalue = mat.coeff (0); 00172 eigenvector [0] = 1.0; 00173 eigenvector [1] = 0.0; 00174 } 00175 else 00176 { 00177 eigenvalue = mat.coeff (2); 00178 eigenvector [0] = 0.0; 00179 eigenvector [1] = 1.0; 00180 } 00181 return; 00182 } 00183 00184 // 0.5 to optimize further calculations 00185 typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3)); 00186 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); 00187 00188 typename Matrix::Scalar temp = trace * trace - determinant; 00189 00190 if (temp < 0) 00191 temp = 0; 00192 00193 eigenvalue = trace - ::std::sqrt (temp); 00194 00195 eigenvector [0] = - mat.coeff (1); 00196 eigenvector [1] = mat.coeff (0) - eigenvalue; 00197 eigenvector.normalize (); 00198 } 00199 00200 /** \brief determine the smallest eigenvalue and its corresponding eigenvector 00201 * \param[in] mat input matrix that needs to be symmetric and positive semi definite 00202 * \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix 00203 * \param[out] eigenvalues the smallest eigenvalue of the input matrix 00204 * \ingroup common 00205 */ 00206 template <typename Matrix, typename Vector> inline void 00207 eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues) 00208 { 00209 // if diagonal matrix, the eigenvalues are the diagonal elements 00210 // and the eigenvectors are not unique, thus set to Identity 00211 if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ()) 00212 { 00213 if (mat.coeff (0) < mat.coeff (3)) 00214 { 00215 eigenvalues.coeffRef (0) = mat.coeff (0); 00216 eigenvalues.coeffRef (1) = mat.coeff (3); 00217 eigenvectors.coeffRef (0) = 1.0; 00218 eigenvectors.coeffRef (1) = 0.0; 00219 eigenvectors.coeffRef (2) = 0.0; 00220 eigenvectors.coeffRef (3) = 1.0; 00221 } 00222 else 00223 { 00224 eigenvalues.coeffRef (0) = mat.coeff (3); 00225 eigenvalues.coeffRef (1) = mat.coeff (0); 00226 eigenvectors.coeffRef (0) = 0.0; 00227 eigenvectors.coeffRef (1) = 1.0; 00228 eigenvectors.coeffRef (2) = 1.0; 00229 eigenvectors.coeffRef (3) = 0.0; 00230 } 00231 return; 00232 } 00233 00234 // 0.5 to optimize further calculations 00235 typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3)); 00236 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1); 00237 00238 typename Matrix::Scalar temp = trace * trace - determinant; 00239 00240 if (temp < 0) 00241 temp = 0; 00242 else 00243 temp = ::std::sqrt (temp); 00244 00245 eigenvalues.coeffRef (0) = trace - temp; 00246 eigenvalues.coeffRef (1) = trace + temp; 00247 00248 // either this is in a row or column depending on RowMajor or ColumnMajor 00249 eigenvectors.coeffRef (0) = - mat.coeff (1); 00250 eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0); 00251 typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0) / 00252 static_cast<typename Matrix::Scalar> (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2))); 00253 eigenvectors.coeffRef (0) *= norm; 00254 eigenvectors.coeffRef (2) *= norm; 00255 eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2); 00256 eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0); 00257 } 00258 00259 /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix 00260 * \param[in] mat symmetric positive semi definite input matrix 00261 * \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed 00262 * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue 00263 * \ingroup common 00264 */ 00265 template<typename Matrix, typename Vector> inline void 00266 computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector) 00267 { 00268 typedef typename Matrix::Scalar Scalar; 00269 // Scale the matrix so its entries are in [-1,1]. The scaling is applied 00270 // only when at least one matrix entry has magnitude larger than 1. 00271 00272 Scalar scale = mat.cwiseAbs ().maxCoeff (); 00273 if (scale <= std::numeric_limits<Scalar>::min ()) 00274 scale = Scalar (1.0); 00275 00276 Matrix scaledMat = mat / scale; 00277 00278 scaledMat.diagonal ().array () -= eigenvalue / scale; 00279 00280 Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); 00281 Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); 00282 Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); 00283 00284 Scalar len1 = vec1.squaredNorm (); 00285 Scalar len2 = vec2.squaredNorm (); 00286 Scalar len3 = vec3.squaredNorm (); 00287 00288 if (len1 >= len2 && len1 >= len3) 00289 eigenvector = vec1 / std::sqrt (len1); 00290 else if (len2 >= len1 && len2 >= len3) 00291 eigenvector = vec2 / std::sqrt (len2); 00292 else 00293 eigenvector = vec3 / std::sqrt (len3); 00294 } 00295 00296 /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix 00297 * \param[in] mat symmetric positive semi definite input matrix 00298 * \param[out] eigenvalue smallest eigenvalue of the input matrix 00299 * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue 00300 * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue. 00301 * \ingroup common 00302 */ 00303 template<typename Matrix, typename Vector> inline void 00304 eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector) 00305 { 00306 typedef typename Matrix::Scalar Scalar; 00307 // Scale the matrix so its entries are in [-1,1]. The scaling is applied 00308 // only when at least one matrix entry has magnitude larger than 1. 00309 00310 Scalar scale = mat.cwiseAbs ().maxCoeff (); 00311 if (scale <= std::numeric_limits<Scalar>::min ()) 00312 scale = Scalar (1.0); 00313 00314 Matrix scaledMat = mat / scale; 00315 00316 Vector eigenvalues; 00317 computeRoots (scaledMat, eigenvalues); 00318 00319 eigenvalue = eigenvalues (0) * scale; 00320 00321 scaledMat.diagonal ().array () -= eigenvalues (0); 00322 00323 Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1)); 00324 Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2)); 00325 Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2)); 00326 00327 Scalar len1 = vec1.squaredNorm (); 00328 Scalar len2 = vec2.squaredNorm (); 00329 Scalar len3 = vec3.squaredNorm (); 00330 00331 if (len1 >= len2 && len1 >= len3) 00332 eigenvector = vec1 / std::sqrt (len1); 00333 else if (len2 >= len1 && len2 >= len3) 00334 eigenvector = vec2 / std::sqrt (len2); 00335 else 00336 eigenvector = vec3 / std::sqrt (len3); 00337 } 00338 00339 /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix 00340 * \param[in] mat symmetric positive semi definite input matrix 00341 * \param[out] evals resulting eigenvalues in ascending order 00342 * \ingroup common 00343 */ 00344 template<typename Matrix, typename Vector> inline void 00345 eigen33 (const Matrix& mat, Vector& evals) 00346 { 00347 typedef typename Matrix::Scalar Scalar; 00348 Scalar scale = mat.cwiseAbs ().maxCoeff (); 00349 if (scale <= std::numeric_limits<Scalar>::min ()) 00350 scale = Scalar (1.0); 00351 00352 Matrix scaledMat = mat / scale; 00353 computeRoots (scaledMat, evals); 00354 evals *= scale; 00355 } 00356 00357 /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix 00358 * \param[in] mat symmetric positive semi definite input matrix 00359 * \param[out] evecs resulting eigenvalues in ascending order 00360 * \param[out] evals corresponding eigenvectors in correct order according to eigenvalues 00361 * \ingroup common 00362 */ 00363 template<typename Matrix, typename Vector> inline void 00364 eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) 00365 { 00366 typedef typename Matrix::Scalar Scalar; 00367 // Scale the matrix so its entries are in [-1,1]. The scaling is applied 00368 // only when at least one matrix entry has magnitude larger than 1. 00369 00370 Scalar scale = mat.cwiseAbs ().maxCoeff (); 00371 if (scale <= std::numeric_limits<Scalar>::min ()) 00372 scale = Scalar (1.0); 00373 00374 Matrix scaledMat = mat / scale; 00375 00376 // Compute the eigenvalues 00377 computeRoots (scaledMat, evals); 00378 00379 if ((evals (2) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon ()) 00380 { 00381 // all three equal 00382 evecs.setIdentity (); 00383 } 00384 else if ((evals (1) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon () ) 00385 { 00386 // first and second equal 00387 Matrix tmp; 00388 tmp = scaledMat; 00389 tmp.diagonal ().array () -= evals (2); 00390 00391 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00392 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00393 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00394 00395 Scalar len1 = vec1.squaredNorm (); 00396 Scalar len2 = vec2.squaredNorm (); 00397 Scalar len3 = vec3.squaredNorm (); 00398 00399 if (len1 >= len2 && len1 >= len3) 00400 evecs.col (2) = vec1 / std::sqrt (len1); 00401 else if (len2 >= len1 && len2 >= len3) 00402 evecs.col (2) = vec2 / std::sqrt (len2); 00403 else 00404 evecs.col (2) = vec3 / std::sqrt (len3); 00405 00406 evecs.col (1) = evecs.col (2).unitOrthogonal (); 00407 evecs.col (0) = evecs.col (1).cross (evecs.col (2)); 00408 } 00409 else if ((evals (2) - evals (1)) <= Eigen::NumTraits<Scalar>::epsilon () ) 00410 { 00411 // second and third equal 00412 Matrix tmp; 00413 tmp = scaledMat; 00414 tmp.diagonal ().array () -= evals (0); 00415 00416 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00417 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00418 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00419 00420 Scalar len1 = vec1.squaredNorm (); 00421 Scalar len2 = vec2.squaredNorm (); 00422 Scalar len3 = vec3.squaredNorm (); 00423 00424 if (len1 >= len2 && len1 >= len3) 00425 evecs.col (0) = vec1 / std::sqrt (len1); 00426 else if (len2 >= len1 && len2 >= len3) 00427 evecs.col (0) = vec2 / std::sqrt (len2); 00428 else 00429 evecs.col (0) = vec3 / std::sqrt (len3); 00430 00431 evecs.col (1) = evecs.col (0).unitOrthogonal (); 00432 evecs.col (2) = evecs.col (0).cross (evecs.col (1)); 00433 } 00434 else 00435 { 00436 Matrix tmp; 00437 tmp = scaledMat; 00438 tmp.diagonal ().array () -= evals (2); 00439 00440 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00441 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00442 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00443 00444 Scalar len1 = vec1.squaredNorm (); 00445 Scalar len2 = vec2.squaredNorm (); 00446 Scalar len3 = vec3.squaredNorm (); 00447 #ifdef _WIN32 00448 Scalar *mmax = new Scalar[3]; 00449 #else 00450 Scalar mmax[3]; 00451 #endif 00452 unsigned int min_el = 2; 00453 unsigned int max_el = 2; 00454 if (len1 >= len2 && len1 >= len3) 00455 { 00456 mmax[2] = len1; 00457 evecs.col (2) = vec1 / std::sqrt (len1); 00458 } 00459 else if (len2 >= len1 && len2 >= len3) 00460 { 00461 mmax[2] = len2; 00462 evecs.col (2) = vec2 / std::sqrt (len2); 00463 } 00464 else 00465 { 00466 mmax[2] = len3; 00467 evecs.col (2) = vec3 / std::sqrt (len3); 00468 } 00469 00470 tmp = scaledMat; 00471 tmp.diagonal ().array () -= evals (1); 00472 00473 vec1 = tmp.row (0).cross (tmp.row (1)); 00474 vec2 = tmp.row (0).cross (tmp.row (2)); 00475 vec3 = tmp.row (1).cross (tmp.row (2)); 00476 00477 len1 = vec1.squaredNorm (); 00478 len2 = vec2.squaredNorm (); 00479 len3 = vec3.squaredNorm (); 00480 if (len1 >= len2 && len1 >= len3) 00481 { 00482 mmax[1] = len1; 00483 evecs.col (1) = vec1 / std::sqrt (len1); 00484 min_el = len1 <= mmax[min_el] ? 1 : min_el; 00485 max_el = len1 > mmax[max_el] ? 1 : max_el; 00486 } 00487 else if (len2 >= len1 && len2 >= len3) 00488 { 00489 mmax[1] = len2; 00490 evecs.col (1) = vec2 / std::sqrt (len2); 00491 min_el = len2 <= mmax[min_el] ? 1 : min_el; 00492 max_el = len2 > mmax[max_el] ? 1 : max_el; 00493 } 00494 else 00495 { 00496 mmax[1] = len3; 00497 evecs.col (1) = vec3 / std::sqrt (len3); 00498 min_el = len3 <= mmax[min_el] ? 1 : min_el; 00499 max_el = len3 > mmax[max_el] ? 1 : max_el; 00500 } 00501 00502 tmp = scaledMat; 00503 tmp.diagonal ().array () -= evals (0); 00504 00505 vec1 = tmp.row (0).cross (tmp.row (1)); 00506 vec2 = tmp.row (0).cross (tmp.row (2)); 00507 vec3 = tmp.row (1).cross (tmp.row (2)); 00508 00509 len1 = vec1.squaredNorm (); 00510 len2 = vec2.squaredNorm (); 00511 len3 = vec3.squaredNorm (); 00512 if (len1 >= len2 && len1 >= len3) 00513 { 00514 mmax[0] = len1; 00515 evecs.col (0) = vec1 / std::sqrt (len1); 00516 min_el = len3 <= mmax[min_el] ? 0 : min_el; 00517 max_el = len3 > mmax[max_el] ? 0 : max_el; 00518 } 00519 else if (len2 >= len1 && len2 >= len3) 00520 { 00521 mmax[0] = len2; 00522 evecs.col (0) = vec2 / std::sqrt (len2); 00523 min_el = len3 <= mmax[min_el] ? 0 : min_el; 00524 max_el = len3 > mmax[max_el] ? 0 : max_el; 00525 } 00526 else 00527 { 00528 mmax[0] = len3; 00529 evecs.col (0) = vec3 / std::sqrt (len3); 00530 min_el = len3 <= mmax[min_el] ? 0 : min_el; 00531 max_el = len3 > mmax[max_el] ? 0 : max_el; 00532 } 00533 00534 unsigned mid_el = 3 - min_el - max_el; 00535 evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized (); 00536 evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized (); 00537 #ifdef _WIN32 00538 delete [] mmax; 00539 #endif 00540 } 00541 // Rescale back to the original size. 00542 evals *= scale; 00543 } 00544 00545 /** \brief Calculate the inverse of a 2x2 matrix 00546 * \param[in] matrix matrix to be inverted 00547 * \param[out] inverse the resultant inverted matrix 00548 * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results 00549 * \return determinant of the original matrix => if 0 no inverse exists => result is invalid 00550 * \ingroup common 00551 */ 00552 template<typename Matrix> inline typename Matrix::Scalar 00553 invert2x2 (const Matrix& matrix, Matrix& inverse) 00554 { 00555 typedef typename Matrix::Scalar Scalar; 00556 Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ; 00557 00558 if (det != 0) 00559 { 00560 //Scalar inv_det = Scalar (1.0) / det; 00561 inverse.coeffRef (0) = matrix.coeff (3); 00562 inverse.coeffRef (1) = - matrix.coeff (1); 00563 inverse.coeffRef (2) = - matrix.coeff (2); 00564 inverse.coeffRef (3) = matrix.coeff (0); 00565 inverse /= det; 00566 } 00567 return det; 00568 } 00569 00570 /** \brief Calculate the inverse of a 3x3 symmetric matrix. 00571 * \param[in] matrix matrix to be inverted 00572 * \param[out] inverse the resultant inverted matrix 00573 * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results 00574 * \return determinant of the original matrix => if 0 no inverse exists => result is invalid 00575 * \ingroup common 00576 */ 00577 template<typename Matrix> inline typename Matrix::Scalar 00578 invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse) 00579 { 00580 typedef typename Matrix::Scalar Scalar; 00581 // elements 00582 // a b c 00583 // b d e 00584 // c e f 00585 //| a b c |-1 | fd-ee ce-bf be-cd | 00586 //| b d e | = 1/det * | ce-bf af-cc bc-ae | 00587 //| c e f | | be-cd bc-ae ad-bb | 00588 00589 //det = a(fd-ee) + b(ec-fb) + c(eb-dc) 00590 00591 Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5); 00592 Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8); 00593 Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4); 00594 00595 Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd; 00596 00597 if (det != 0) 00598 { 00599 //Scalar inv_det = Scalar (1.0) / det; 00600 inverse.coeffRef (0) = fd_ee; 00601 inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf; 00602 inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd; 00603 inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2)); 00604 inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5)); 00605 inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1)); 00606 inverse /= det; 00607 } 00608 return det; 00609 } 00610 00611 /** \brief Calculate the inverse of a general 3x3 matrix. 00612 * \param[in] matrix matrix to be inverted 00613 * \param[out] inverse the resultant inverted matrix 00614 * \return determinant of the original matrix => if 0 no inverse exists => result is invalid 00615 * \ingroup common 00616 */ 00617 template<typename Matrix> inline typename Matrix::Scalar 00618 invert3x3Matrix (const Matrix& matrix, Matrix& inverse) 00619 { 00620 typedef typename Matrix::Scalar Scalar; 00621 00622 //| a b c |-1 | ie-hf hc-ib fb-ec | 00623 //| d e f | = 1/det * | gf-id ia-gc dc-fa | 00624 //| g h i | | hd-ge gb-ha ea-db | 00625 //det = a(ie-hf) + d(hc-ib) + g(fb-ec) 00626 00627 Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5); 00628 Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1); 00629 Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2); 00630 Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ; 00631 00632 if (det != 0) 00633 { 00634 inverse.coeffRef (0) = ie_hf; 00635 inverse.coeffRef (1) = hc_ib; 00636 inverse.coeffRef (2) = fb_ec; 00637 inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3); 00638 inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2); 00639 inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0); 00640 inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4); 00641 inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0); 00642 inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1); 00643 00644 inverse /= det; 00645 } 00646 return det; 00647 } 00648 00649 template<typename Matrix> inline typename Matrix::Scalar 00650 determinant3x3Matrix (const Matrix& matrix) 00651 { 00652 // result is independent of Row/Col Major storage! 00653 return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) + 00654 matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) + 00655 matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ; 00656 } 00657 00658 /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector 00659 * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00660 * \param[in] z_axis the z-axis 00661 * \param[in] y_direction the y direction 00662 * \param[out] transformation the resultant 3D rotation 00663 * \ingroup common 00664 */ 00665 inline void 00666 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 00667 const Eigen::Vector3f& y_direction, 00668 Eigen::Affine3f& transformation); 00669 00670 /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector 00671 * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00672 * \param[in] z_axis the z-axis 00673 * \param[in] y_direction the y direction 00674 * \return the resultant 3D rotation 00675 * \ingroup common 00676 */ 00677 inline Eigen::Affine3f 00678 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 00679 const Eigen::Vector3f& y_direction); 00680 00681 /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector 00682 * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00683 * \param[in] x_axis the x-axis 00684 * \param[in] y_direction the y direction 00685 * \param[out] transformation the resultant 3D rotation 00686 * \ingroup common 00687 */ 00688 inline void 00689 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 00690 const Eigen::Vector3f& y_direction, 00691 Eigen::Affine3f& transformation); 00692 00693 /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector 00694 * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00695 * \param[in] x_axis the x-axis 00696 * \param[in] y_direction the y direction 00697 * \return the resulting 3D rotation 00698 * \ingroup common 00699 */ 00700 inline Eigen::Affine3f 00701 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 00702 const Eigen::Vector3f& y_direction); 00703 00704 /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector 00705 * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00706 * \param[in] y_direction the y direction 00707 * \param[in] z_axis the z-axis 00708 * \param[out] transformation the resultant 3D rotation 00709 * \ingroup common 00710 */ 00711 inline void 00712 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 00713 const Eigen::Vector3f& z_axis, 00714 Eigen::Affine3f& transformation); 00715 00716 /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector 00717 * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00718 * \param[in] y_direction the y direction 00719 * \param[in] z_axis the z-axis 00720 * \return transformation the resultant 3D rotation 00721 * \ingroup common 00722 */ 00723 inline Eigen::Affine3f 00724 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 00725 const Eigen::Vector3f& z_axis); 00726 00727 /** \brief Get the transformation that will translate \a orign to (0,0,0) and rotate \a z_axis into (0,0,1) 00728 * and \a y_direction into a vector with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis) 00729 * \param[in] y_direction the y direction 00730 * \param[in] z_axis the z-axis 00731 * \param[in] origin the origin 00732 * \param[in] transformation the resultant transformation matrix 00733 * \ingroup common 00734 */ 00735 inline void 00736 getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, 00737 const Eigen::Vector3f& z_axis, 00738 const Eigen::Vector3f& origin, 00739 Eigen::Affine3f& transformation); 00740 00741 /** \brief Extract the Euler angles (XYZ-convention) from the given transformation 00742 * \param[in] t the input transformation matrix 00743 * \param[in] roll the resulting roll angle 00744 * \param[in] pitch the resulting pitch angle 00745 * \param[in] yaw the resulting yaw angle 00746 * \ingroup common 00747 */ 00748 inline void 00749 getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw); 00750 00751 /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation 00752 * \param[in] t the input transformation matrix 00753 * \param[out] x the resulting x translation 00754 * \param[out] y the resulting y translation 00755 * \param[out] z the resulting z translation 00756 * \param[out] roll the resulting roll angle 00757 * \param[out] pitch the resulting pitch angle 00758 * \param[out] yaw the resulting yaw angle 00759 * \ingroup common 00760 */ 00761 inline void 00762 getTranslationAndEulerAngles (const Eigen::Affine3f& t, 00763 float& x, float& y, float& z, 00764 float& roll, float& pitch, float& yaw); 00765 00766 /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention) 00767 * \param[in] x the input x translation 00768 * \param[in] y the input y translation 00769 * \param[in] z the input z translation 00770 * \param[in] roll the input roll angle 00771 * \param[in] pitch the input pitch angle 00772 * \param[in] yaw the input yaw angle 00773 * \param[out] t the resulting transformation matrix 00774 * \ingroup common 00775 */ 00776 template <typename Scalar> inline void 00777 getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, 00778 Eigen::Transform<Scalar, 3, Eigen::Affine> &t); 00779 00780 inline void 00781 getTransformation (float x, float y, float z, float roll, float pitch, float yaw, 00782 Eigen::Affine3f &t) 00783 { 00784 return (getTransformation<float> (x, y, z, roll, pitch, yaw, t)); 00785 } 00786 00787 inline void 00788 getTransformation (double x, double y, double z, double roll, double pitch, double yaw, 00789 Eigen::Affine3d &t) 00790 { 00791 return (getTransformation<double> (x, y, z, roll, pitch, yaw, t)); 00792 } 00793 00794 /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention) 00795 * \param[in] x the input x translation 00796 * \param[in] y the input y translation 00797 * \param[in] z the input z translation 00798 * \param[in] roll the input roll angle 00799 * \param[in] pitch the input pitch angle 00800 * \param[in] yaw the input yaw angle 00801 * \return the resulting transformation matrix 00802 * \ingroup common 00803 */ 00804 inline Eigen::Affine3f 00805 getTransformation (float x, float y, float z, float roll, float pitch, float yaw); 00806 00807 /** \brief Write a matrix to an output stream 00808 * \param[in] matrix the matrix to output 00809 * \param[out] file the output stream 00810 * \ingroup common 00811 */ 00812 template <typename Derived> void 00813 saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file); 00814 00815 /** \brief Read a matrix from an input stream 00816 * \param[out] matrix the resulting matrix, read from the input stream 00817 * \param[in,out] file the input stream 00818 * \ingroup common 00819 */ 00820 template <typename Derived> void 00821 loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file); 00822 00823 // PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1, 00824 // followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over 00825 // finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3. 00826 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \ 00827 : (int (a) == 1 || int (b) == 1) ? 1 \ 00828 : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \ 00829 : (int (a) <= int (b)) ? int (a) : int (b)) 00830 00831 /** \brief Returns the transformation between two point sets. 00832 * The algorithm is based on: 00833 * "Least-squares estimation of transformation parameters between two point patterns", 00834 * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 00835 * 00836 * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that 00837 * \f{align*} 00838 * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2 00839 * \f} 00840 * is minimized. 00841 * 00842 * The algorithm is based on the analysis of the covariance matrix 00843 * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$ 00844 * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where 00845 * \f$d\f$ is corresponding to the dimension (which is typically small). 00846 * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$ 00847 * though the actual computational effort lies in the covariance 00848 * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when 00849 * the input point sets have dimension \f$d \times m\f$. 00850 * 00851 * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$ 00852 * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$. 00853 * \param[in] with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. (default: false) 00854 * \return The homogeneous transformation 00855 * \f{align*} 00856 * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} 00857 * \f} 00858 * minimizing the resudiual above. This transformation is always returned as an 00859 * Eigen::Matrix. 00860 */ 00861 template <typename Derived, typename OtherDerived> 00862 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type 00863 umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling = false); 00864 } 00865 00866 #include <pcl/common/impl/eigen.hpp> 00867 00868 #if defined __SUNPRO_CC 00869 # pragma enable_warn 00870 #endif 00871 00872 #endif //PCL_COMMON_EIGEN_H_