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, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_COMMON_EIGEN_IMPL_HPP_ 00040 #define PCL_COMMON_EIGEN_IMPL_HPP_ 00041 00042 #include <pcl/pcl_macros.h> 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////// 00045 void 00046 pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 00047 const Eigen::Vector3f& y_direction, 00048 Eigen::Affine3f& transformation) 00049 { 00050 Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized(); 00051 Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized(); 00052 Eigen::Vector3f tmp2 = z_axis.normalized(); 00053 00054 transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; 00055 transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; 00056 transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; 00057 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f; 00058 } 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////// 00061 Eigen::Affine3f 00062 pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, 00063 const Eigen::Vector3f& y_direction) 00064 { 00065 Eigen::Affine3f transformation; 00066 getTransFromUnitVectorsZY (z_axis, y_direction, transformation); 00067 return (transformation); 00068 } 00069 00070 ////////////////////////////////////////////////////////////////////////////////////////// 00071 void 00072 pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 00073 const Eigen::Vector3f& y_direction, 00074 Eigen::Affine3f& transformation) 00075 { 00076 Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized(); 00077 Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized(); 00078 Eigen::Vector3f tmp0 = x_axis.normalized(); 00079 00080 transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; 00081 transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; 00082 transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; 00083 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f; 00084 } 00085 00086 ////////////////////////////////////////////////////////////////////////////////////////// 00087 Eigen::Affine3f 00088 pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, 00089 const Eigen::Vector3f& y_direction) 00090 { 00091 Eigen::Affine3f transformation; 00092 getTransFromUnitVectorsXY (x_axis, y_direction, transformation); 00093 return (transformation); 00094 } 00095 00096 ////////////////////////////////////////////////////////////////////////////////////////// 00097 void 00098 pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 00099 const Eigen::Vector3f& z_axis, 00100 Eigen::Affine3f& transformation) 00101 { 00102 getTransFromUnitVectorsZY (z_axis, y_direction, transformation); 00103 } 00104 00105 ////////////////////////////////////////////////////////////////////////////////////////// 00106 Eigen::Affine3f 00107 pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, 00108 const Eigen::Vector3f& z_axis) 00109 { 00110 Eigen::Affine3f transformation; 00111 getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation); 00112 return (transformation); 00113 } 00114 00115 void 00116 pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, 00117 const Eigen::Vector3f& z_axis, 00118 const Eigen::Vector3f& origin, 00119 Eigen::Affine3f& transformation) 00120 { 00121 getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation); 00122 Eigen::Vector3f translation = transformation*origin; 00123 transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2]; 00124 } 00125 00126 ////////////////////////////////////////////////////////////////////////////////////////// 00127 void 00128 pcl::getEulerAngles (const Eigen::Affine3f& t, float& roll, float& pitch, float& yaw) 00129 { 00130 roll = atan2f(t(2,1), t(2,2)); 00131 pitch = asinf(-t(2,0)); 00132 yaw = atan2f(t(1,0), t(0,0)); 00133 } 00134 00135 ////////////////////////////////////////////////////////////////////////////////////////// 00136 void 00137 pcl::getTranslationAndEulerAngles (const Eigen::Affine3f& t, 00138 float& x, float& y, float& z, 00139 float& roll, float& pitch, float& yaw) 00140 { 00141 x = t(0,3); 00142 y = t(1,3); 00143 z = t(2,3); 00144 roll = atan2f(t(2,1), t(2,2)); 00145 pitch = asinf(-t(2,0)); 00146 yaw = atan2f(t(1,0), t(0,0)); 00147 } 00148 00149 ////////////////////////////////////////////////////////////////////////////////////////// 00150 template <typename Scalar> void 00151 pcl::getTransformation (Scalar x, Scalar y, Scalar z, 00152 Scalar roll, Scalar pitch, Scalar yaw, 00153 Eigen::Transform<Scalar, 3, Eigen::Affine> &t) 00154 { 00155 Scalar A = cos (yaw), B = sin (yaw), C = cos (pitch), D = sin (pitch), 00156 E = cos (roll), F = sin (roll), DE = D*E, DF = D*F; 00157 00158 t (0, 0) = A*C; t (0, 1) = A*DF - B*E; t (0, 2) = B*F + A*DE; t (0, 3) = x; 00159 t (1, 0) = B*C; t (1, 1) = A*E + B*DF; t (1, 2) = B*DE - A*F; t (1, 3) = y; 00160 t (2, 0) = -D; t (2, 1) = C*F; t (2, 2) = C*E; t (2, 3) = z; 00161 t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1; 00162 } 00163 00164 ////////////////////////////////////////////////////////////////////////////////////////// 00165 Eigen::Affine3f 00166 pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw) 00167 { 00168 Eigen::Affine3f t; 00169 getTransformation (x, y, z, roll, pitch, yaw, t); 00170 return (t); 00171 } 00172 00173 ////////////////////////////////////////////////////////////////////////////////////////// 00174 template <typename Derived> void 00175 pcl::saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file) 00176 { 00177 uint32_t rows = static_cast<uint32_t> (matrix.rows ()), cols = static_cast<uint32_t> (matrix.cols ()); 00178 file.write (reinterpret_cast<char*> (&rows), sizeof (rows)); 00179 file.write (reinterpret_cast<char*> (&cols), sizeof (cols)); 00180 for (uint32_t i = 0; i < rows; ++i) 00181 for (uint32_t j = 0; j < cols; ++j) 00182 { 00183 typename Derived::Scalar tmp = matrix(i,j); 00184 file.write (reinterpret_cast<const char*> (&tmp), sizeof (tmp)); 00185 } 00186 } 00187 00188 ////////////////////////////////////////////////////////////////////////////////////////// 00189 template <typename Derived> void 00190 pcl::loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file) 00191 { 00192 Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_); 00193 00194 uint32_t rows, cols; 00195 file.read (reinterpret_cast<char*> (&rows), sizeof (rows)); 00196 file.read (reinterpret_cast<char*> (&cols), sizeof (cols)); 00197 if (matrix.rows () != static_cast<int>(rows) || matrix.cols () != static_cast<int>(cols)) 00198 matrix.derived().resize(rows, cols); 00199 00200 for (uint32_t i = 0; i < rows; ++i) 00201 for (uint32_t j = 0; j < cols; ++j) 00202 { 00203 typename Derived::Scalar tmp; 00204 file.read (reinterpret_cast<char*> (&tmp), sizeof (tmp)); 00205 matrix (i, j) = tmp; 00206 } 00207 } 00208 00209 ////////////////////////////////////////////////////////////////////////////////////////// 00210 template <typename Derived, typename OtherDerived> 00211 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type 00212 pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling) 00213 { 00214 typedef typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; 00215 typedef typename Eigen::internal::traits<TransformationMatrixType>::Scalar Scalar; 00216 typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; 00217 typedef typename Derived::Index Index; 00218 00219 EIGEN_STATIC_ASSERT (!Eigen::NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) 00220 EIGEN_STATIC_ASSERT ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value), 00221 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 00222 00223 enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; 00224 00225 typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType; 00226 typedef Eigen::Matrix<Scalar, Dimension, Dimension> MatrixType; 00227 typedef typename Eigen::internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType; 00228 00229 const Index m = src.rows (); // dimension 00230 const Index n = src.cols (); // number of measurements 00231 00232 // required for demeaning ... 00233 const RealScalar one_over_n = 1 / static_cast<RealScalar> (n); 00234 00235 // computation of mean 00236 const VectorType src_mean = src.rowwise ().sum () * one_over_n; 00237 const VectorType dst_mean = dst.rowwise ().sum () * one_over_n; 00238 00239 // demeaning of src and dst points 00240 const RowMajorMatrixType src_demean = src.colwise () - src_mean; 00241 const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean; 00242 00243 // Eq. (36)-(37) 00244 const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n; 00245 00246 // Eq. (38) 00247 const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ()); 00248 00249 Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV); 00250 00251 // Initialize the resulting transformation with an identity matrix... 00252 TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1); 00253 00254 // Eq. (39) 00255 VectorType S = VectorType::Ones (m); 00256 if (sigma.determinant () < 0) 00257 S (m - 1) = -1; 00258 00259 // Eq. (40) and (43) 00260 const VectorType& d = svd.singularValues (); 00261 Index rank = 0; 00262 for (Index i = 0; i < m; ++i) 00263 if (!Eigen::internal::isMuchSmallerThan (d.coeff (i), d.coeff (0))) 00264 ++rank; 00265 if (rank == m - 1) 00266 { 00267 if (svd.matrixU ().determinant () * svd.matrixV ().determinant () > 0) 00268 Rt.block (0, 0, m, m).noalias () = svd.matrixU () * svd.matrixV ().transpose (); 00269 else 00270 { 00271 const Scalar s = S (m - 1); 00272 S (m - 1) = -1; 00273 Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); 00274 S (m - 1) = s; 00275 } 00276 } 00277 else 00278 { 00279 Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose (); 00280 } 00281 00282 // Eq. (42) 00283 if (with_scaling) 00284 { 00285 // Eq. (42) 00286 const Scalar c = 1 / src_var * svd.singularValues ().dot (S); 00287 00288 // Eq. (41) 00289 Rt.col (m).head (m) = dst_mean; 00290 Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean; 00291 Rt.block (0, 0, m, m) *= c; 00292 } 00293 else 00294 { 00295 Rt.col (m).head (m) = dst_mean; 00296 Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean; 00297 } 00298 00299 return (Rt); 00300 } 00301 00302 #endif //PCL_COMMON_EIGEN_IMPL_HPP_ 00303