Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: eigen.h 2517 2011-09-19 17:47:45Z jspricke $ 00035 * 00036 */ 00037 // This file is part of Eigen, a lightweight C++ template library 00038 // for linear algebra. 00039 // 00040 // Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr> 00041 // 00042 // Eigen is free software; you can redistribute it and/or 00043 // modify it under the terms of the GNU Lesser General Public 00044 // License as published by the Free Software Foundation; either 00045 // version 3 of the License, or (at your option) any later version. 00046 // 00047 // Alternatively, you can redistribute it and/or 00048 // modify it under the terms of the GNU General Public License as 00049 // published by the Free Software Foundation; either version 2 of 00050 // the License, or (at your option) any later version. 00051 // 00052 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY 00053 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 00054 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the 00055 // GNU General Public License for more details. 00056 // 00057 // You should have received a copy of the GNU Lesser General Public 00058 // License and a copy of the GNU General Public License along with 00059 // Eigen. If not, see <http://www.gnu.org/licenses/>. 00060 00061 // The computeRoots function included in this is based on materials 00062 // covered by the following copyright and license: 00063 // 00064 // Geometric Tools, LLC 00065 // Copyright (c) 1998-2010 00066 // Distributed under the Boost Software License, Version 1.0. 00067 // 00068 // Permission is hereby granted, free of charge, to any person or organization 00069 // obtaining a copy of the software and accompanying documentation covered by 00070 // this license (the "Software") to use, reproduce, display, distribute, 00071 // execute, and transmit the Software, and to prepare derivative works of the 00072 // Software, and to permit third-parties to whom the Software is furnished to 00073 // do so, all subject to the following: 00074 // 00075 // The copyright notices in the Software and this entire statement, including 00076 // the above license grant, this restriction and the following disclaimer, 00077 // must be included in all copies of the Software, in whole or in part, and 00078 // all derivative works of the Software, unless such copies or derivative 00079 // works are solely in the form of machine-executable object code generated by 00080 // a source language processor. 00081 // 00082 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00083 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00084 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 00085 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 00086 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 00087 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 00088 // DEALINGS IN THE SOFTWARE. 00089 00090 #ifndef PCL_EIGEN_H_ 00091 #define PCL_EIGEN_H_ 00092 00093 #define NOMINMAX 00094 00095 #include <Eigen/Core> 00096 #include <Eigen/Eigenvalues> 00097 #include <Eigen/Geometry> 00098 00099 namespace pcl 00100 { 00101 template<typename Scalar, typename Roots> inline void computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots) 00102 { 00103 roots(0) = Scalar(0); 00104 Scalar d = b * b - 4.0 * c; 00105 if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN! 00106 d = 0.0; 00107 00108 Scalar sd = sqrt (d); 00109 00110 roots (2) = 0.5f * (b + sd); 00111 roots (1) = 0.5f * (b - sd); 00112 } 00113 00114 template<typename Matrix, typename Roots> inline void 00115 computeRoots (const Matrix& m, Roots& roots) 00116 { 00117 typedef typename Matrix::Scalar Scalar; 00118 00119 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The 00120 // eigenvalues are the roots to this equation, all guaranteed to be 00121 // real-valued, because the matrix is symmetric. 00122 Scalar c0 = m(0,0)*m(1,1)*m(2,2) 00123 + Scalar(2) * m(0,1)*m(0,2)*m(1,2) 00124 - m(0,0)*m(1,2)*m(1,2) 00125 - m(1,1)*m(0,2)*m(0,2) 00126 - m(2,2)*m(0,1)*m(0,1); 00127 Scalar c1 = m(0,0)*m(1,1) - 00128 m(0,1)*m(0,1) + 00129 m(0,0)*m(2,2) - 00130 m(0,2)*m(0,2) + 00131 m(1,1)*m(2,2) - 00132 m(1,2)*m(1,2); 00133 Scalar c2 = m(0,0) + m(1,1) + m(2,2); 00134 00135 00136 if (fabs(c0) < Eigen::NumTraits<Scalar>::epsilon())// one root is 0 -> quadratic equation 00137 computeRoots2 (c2, c1, roots); 00138 else 00139 { 00140 const Scalar s_inv3 = 1.0/3.0; 00141 const Scalar s_sqrt3 = Eigen::internal::sqrt (Scalar (3.0)); 00142 // Construct the parameters used in classifying the roots of the equation 00143 // and in solving the equation for the roots in closed form. 00144 Scalar c2_over_3 = c2*s_inv3; 00145 Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3; 00146 if (a_over_3 > Scalar (0)) 00147 a_over_3 = Scalar (0); 00148 00149 Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1)); 00150 00151 Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3; 00152 if (q > Scalar(0)) 00153 q = Scalar(0); 00154 00155 // Compute the eigenvalues by solving for the roots of the polynomial. 00156 Scalar rho = Eigen::internal::sqrt (-a_over_3); 00157 Scalar theta = std::atan2 (Eigen::internal::sqrt (-q), half_b)*s_inv3; 00158 Scalar cos_theta = Eigen::internal::cos (theta); 00159 Scalar sin_theta = Eigen::internal::sin (theta); 00160 roots(0) = c2_over_3 + Scalar(2) * rho * cos_theta; 00161 roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta); 00162 roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta); 00163 00164 // Sort in increasing order. 00165 if (roots (0) >= roots (1)) 00166 std::swap (roots (0), roots (1)); 00167 if (roots (1) >= roots (2)) 00168 { 00169 std::swap (roots (1), roots (2)); 00170 if (roots (0) >= roots (1)) 00171 std::swap (roots (0), roots (1)); 00172 } 00173 00174 if (roots(0) <= 0) // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0 00175 computeRoots2 (c2, c1, roots); 00176 } 00177 } 00178 00179 template<typename Matrix, typename Vector> void 00180 eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals) 00181 { 00182 typedef typename Matrix::Scalar Scalar; 00183 // Scale the matrix so its entries are in [-1,1]. The scaling is applied 00184 // only when at least one matrix entry has magnitude larger than 1. 00185 00186 Scalar scale = mat.cwiseAbs ().maxCoeff (); 00187 if (scale <= std::numeric_limits<Scalar>::min()) 00188 scale = Scalar(1.0); 00189 00190 Matrix scaledMat = mat / scale; 00191 00192 // Compute the eigenvalues 00193 computeRoots (scaledMat,evals); 00194 00195 if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon()) 00196 { 00197 // all three equal 00198 evecs.setIdentity(); 00199 } 00200 else if ((evals(1)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon() ) 00201 { 00202 // first and second equal 00203 Matrix tmp; 00204 tmp = scaledMat; 00205 tmp.diagonal ().array () -= evals (2); 00206 00207 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00208 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00209 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00210 00211 Scalar len1 = vec1.squaredNorm(); 00212 Scalar len2 = vec2.squaredNorm(); 00213 Scalar len3 = vec3.squaredNorm(); 00214 00215 if (len1 >= len2 && len1 >= len3) 00216 evecs.col (2) = vec1 / Eigen::internal::sqrt (len1); 00217 else if (len2 >= len1 && len2 >= len3) 00218 evecs.col (2) = vec2 / Eigen::internal::sqrt (len2); 00219 else 00220 evecs.col (2) = vec3 / Eigen::internal::sqrt (len3); 00221 00222 evecs.col (1) = evecs.col (2).unitOrthogonal (); 00223 evecs.col (0) = evecs.col (1).cross (evecs.col (2)); 00224 } 00225 else if ((evals(2)-evals(1))<=Eigen::NumTraits<Scalar>::epsilon() ) 00226 { 00227 // second and third equal 00228 Matrix tmp; 00229 tmp = scaledMat; 00230 tmp.diagonal ().array () -= evals (0); 00231 00232 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00233 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00234 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00235 00236 Scalar len1 = vec1.squaredNorm(); 00237 Scalar len2 = vec2.squaredNorm(); 00238 Scalar len3 = vec3.squaredNorm(); 00239 00240 if (len1 >= len2 && len1 >= len3) 00241 evecs.col (0) = vec1 / Eigen::internal::sqrt (len1); 00242 else if (len2 >= len1 && len2 >= len3) 00243 evecs.col (0) = vec2 / Eigen::internal::sqrt (len2); 00244 else 00245 evecs.col (0) = vec3 / Eigen::internal::sqrt (len3); 00246 00247 evecs.col (1) = evecs.col (0).unitOrthogonal (); 00248 evecs.col (2) = evecs.col (0).cross (evecs.col (1)); 00249 } 00250 else 00251 { 00252 Matrix tmp; 00253 tmp = scaledMat; 00254 tmp.diagonal ().array () -= evals (2); 00255 00256 Vector vec1 = tmp.row (0).cross (tmp.row (1)); 00257 Vector vec2 = tmp.row (0).cross (tmp.row (2)); 00258 Vector vec3 = tmp.row (1).cross (tmp.row (2)); 00259 00260 Scalar len1 = vec1.squaredNorm (); 00261 Scalar len2 = vec2.squaredNorm (); 00262 Scalar len3 = vec3.squaredNorm (); 00263 #ifdef _WIN32 00264 Scalar *mmax = new Scalar[3]; 00265 #else 00266 Scalar mmax[3]; 00267 #endif 00268 unsigned int min_el = 2; 00269 unsigned int max_el = 2; 00270 if (len1 >= len2 && len1 >= len3) 00271 { 00272 mmax[2] = len1; 00273 evecs.col (2) = vec1 / Eigen::internal::sqrt (len1); 00274 } 00275 else if (len2 >= len1 && len2 >= len3) 00276 { 00277 mmax[2] = len2; 00278 evecs.col (2) = vec2 / Eigen::internal::sqrt (len2); 00279 } 00280 else 00281 { 00282 mmax[2] = len3; 00283 evecs.col (2) = vec3 / Eigen::internal::sqrt (len3); 00284 } 00285 00286 tmp = scaledMat; 00287 tmp.diagonal ().array () -= evals (1); 00288 00289 vec1 = tmp.row (0).cross (tmp.row (1)); 00290 vec2 = tmp.row (0).cross (tmp.row (2)); 00291 vec3 = tmp.row (1).cross (tmp.row (2)); 00292 00293 len1 = vec1.squaredNorm (); 00294 len2 = vec2.squaredNorm (); 00295 len3 = vec3.squaredNorm (); 00296 if (len1 >= len2 && len1 >= len3) 00297 { 00298 mmax[1] = len1; 00299 evecs.col (1) = vec1 / Eigen::internal::sqrt (len1); 00300 min_el = len1 <= mmax[min_el]? 1: min_el; 00301 max_el = len1 > mmax[max_el]? 1: max_el; 00302 } 00303 else if (len2 >= len1 && len2 >= len3) 00304 { 00305 mmax[1] = len2; 00306 evecs.col (1) = vec2 / Eigen::internal::sqrt (len2); 00307 min_el = len2 <= mmax[min_el]? 1: min_el; 00308 max_el = len2 > mmax[max_el]? 1: max_el; 00309 } 00310 else 00311 { 00312 mmax[1] = len3; 00313 evecs.col (1) = vec3 / Eigen::internal::sqrt (len3); 00314 min_el = len3 <= mmax[min_el]? 1: min_el; 00315 max_el = len3 > mmax[max_el]? 1: max_el; 00316 } 00317 00318 tmp = scaledMat; 00319 tmp.diagonal ().array () -= evals (0); 00320 00321 vec1 = tmp.row (0).cross (tmp.row (1)); 00322 vec2 = tmp.row (0).cross (tmp.row (2)); 00323 vec3 = tmp.row (1).cross (tmp.row (2)); 00324 00325 len1 = vec1.squaredNorm (); 00326 len2 = vec2.squaredNorm (); 00327 len3 = vec3.squaredNorm (); 00328 if (len1 >= len2 && len1 >= len3) 00329 { 00330 mmax[0] = len1; 00331 evecs.col (0) = vec1 / Eigen::internal::sqrt (len1); 00332 min_el = len3 <= mmax[min_el]? 0: min_el; 00333 max_el = len3 > mmax[max_el]? 0: max_el; 00334 } 00335 else if (len2 >= len1 && len2 >= len3) 00336 { 00337 mmax[0] = len2; 00338 evecs.col (0) = vec2 / Eigen::internal::sqrt (len2); 00339 min_el = len3 <= mmax[min_el]? 0: min_el; 00340 max_el = len3 > mmax[max_el]? 0: max_el; 00341 } 00342 else 00343 { 00344 mmax[0] = len3; 00345 evecs.col (0) = vec3 / Eigen::internal::sqrt (len3); 00346 min_el = len3 <= mmax[min_el]? 0: min_el; 00347 max_el = len3 > mmax[max_el]? 0: max_el; 00348 } 00349 00350 unsigned mid_el = 3 - min_el - max_el; 00351 evecs.col (min_el) = evecs.col ((min_el+1)%3).cross ( evecs.col ((min_el+2)%3) ).normalized (); 00352 evecs.col (mid_el) = evecs.col ((mid_el+1)%3).cross ( evecs.col ((mid_el+2)%3) ).normalized (); 00353 #ifdef _WIN32 00354 delete [] mmax; 00355 #endif 00356 } 00357 // Rescale back to the original size. 00358 evals *= scale; 00359 } 00360 00368 inline void 00369 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction, 00370 Eigen::Affine3f& transformation); 00371 00379 inline Eigen::Affine3f 00380 getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction); 00381 00389 inline void 00390 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction, 00391 Eigen::Affine3f& transformation); 00392 00400 inline Eigen::Affine3f 00401 getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction); 00402 00404 inline void 00405 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis, 00406 Eigen::Affine3f& transformation); 00407 00409 inline Eigen::Affine3f 00410 getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis); 00411 00420 inline void 00421 getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis, 00422 const Eigen::Vector3f& origin, Eigen::Affine3f& transformation); 00423 00431 inline void 00432 getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw); 00433 00444 inline void 00445 getTranslationAndEulerAngles (const Eigen::Affine3f& t, float& x, float& y, float& z, 00446 float& roll, float& pitch, float& yaw); 00447 00458 inline void 00459 getTransformation (float x, float y, float z, float roll, float pitch, float yaw, Eigen::Affine3f& t); 00460 00471 inline Eigen::Affine3f 00472 getTransformation (float x, float y, float z, float roll, float pitch, float yaw); 00473 00479 template <typename Derived> void 00480 saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file); 00481 00487 template <typename Derived> void 00488 loadBinary (Eigen::MatrixBase<Derived>& matrix, std::istream& file); 00489 } 00490 00491 #include "pcl/common/impl/eigen.hpp" 00492 00493 #endif //#ifndef PCL_EIGEN_H_