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) 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 #ifndef PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_ 00039 #define PCL_COMMON_VECTOR_AVERAGE_IMPL_HPP_ 00040 00041 namespace pcl 00042 { 00043 template <typename real, int dimension> 00044 VectorAverage<real, dimension>::VectorAverage () : 00045 noOfSamples_ (0), accumulatedWeight_ (0), 00046 mean_ (Eigen::Matrix<real, dimension, 1>::Identity ()), 00047 covariance_ (Eigen::Matrix<real, dimension, dimension>::Identity ()) 00048 { 00049 reset(); 00050 } 00051 00052 template <typename real, int dimension> 00053 inline void VectorAverage<real, dimension>::reset() 00054 { 00055 noOfSamples_ = 0; 00056 accumulatedWeight_ = 0.0; 00057 mean_.fill(0); 00058 covariance_.fill(0); 00059 } 00060 00061 template <typename real, int dimension> 00062 inline void VectorAverage<real, dimension>::add(const Eigen::Matrix<real, dimension, 1>& sample, real weight) { 00063 if (weight == 0.0f) 00064 return; 00065 00066 ++noOfSamples_; 00067 accumulatedWeight_ += weight; 00068 real alpha = weight/accumulatedWeight_; 00069 00070 Eigen::Matrix<real, dimension, 1> diff = sample - mean_; 00071 covariance_ = (covariance_ + (diff * diff.transpose())*alpha)*(1.0f-alpha); 00072 00073 mean_ += (diff)*alpha; 00074 00075 //if (pcl_isnan(covariance_(0,0))) 00076 //{ 00077 //cout << PVARN(weight); 00078 //exit(0); 00079 //} 00080 } 00081 00082 template <typename real, int dimension> 00083 inline void VectorAverage<real, dimension>::doPCA(Eigen::Matrix<real, dimension, 1>& eigen_values, Eigen::Matrix<real, dimension, 1>& eigen_vector1, 00084 Eigen::Matrix<real, dimension, 1>& eigen_vector2, Eigen::Matrix<real, dimension, 1>& eigen_vector3) const 00085 { 00086 // The following step is necessary for cases where the values in the covariance matrix are small 00087 // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. 00088 //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>(); 00089 //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance); 00090 //eigen_values = ei_symm.eigenvalues().template cast<real>(); 00091 //Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors().template cast<real>(); 00092 00093 //cout << "My covariance is \n"<<covariance_<<"\n"; 00094 //cout << "My mean is \n"<<mean_<<"\n"; 00095 //cout << "My Eigenvectors \n"<<eigen_vectors<<"\n"; 00096 00097 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_); 00098 eigen_values = ei_symm.eigenvalues(); 00099 Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors(); 00100 00101 eigen_vector1 = eigen_vectors.col(0); 00102 eigen_vector2 = eigen_vectors.col(1); 00103 eigen_vector3 = eigen_vectors.col(2); 00104 } 00105 00106 template <typename real, int dimension> 00107 inline void VectorAverage<real, dimension>::doPCA(Eigen::Matrix<real, dimension, 1>& eigen_values) const 00108 { 00109 // The following step is necessary for cases where the values in the covariance matrix are small 00110 // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. 00111 //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>(); 00112 //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance, false); 00113 //eigen_values = ei_symm.eigenvalues().template cast<real>(); 00114 00115 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_, false); 00116 eigen_values = ei_symm.eigenvalues(); 00117 } 00118 00119 template <typename real, int dimension> 00120 inline void VectorAverage<real, dimension>::getEigenVector1(Eigen::Matrix<real, dimension, 1>& eigen_vector1) const 00121 { 00122 // The following step is necessary for cases where the values in the covariance matrix are small 00123 // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors. 00124 //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>(); 00125 //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance); 00126 //eigen_values = ei_symm.eigenvalues().template cast<real>(); 00127 //Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors().template cast<real>(); 00128 00129 //cout << "My covariance is \n"<<covariance_<<"\n"; 00130 //cout << "My mean is \n"<<mean_<<"\n"; 00131 //cout << "My Eigenvectors \n"<<eigen_vectors<<"\n"; 00132 00133 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_); 00134 Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors(); 00135 eigen_vector1 = eigen_vectors.col(0); 00136 } 00137 00138 00139 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00140 // Special cases for real=float & dimension=3 -> Partial specialization does not work with class templates. :( // 00141 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00142 /////////// 00143 // float // 00144 /////////// 00145 template <> 00146 inline void VectorAverage<float, 3>::doPCA(Eigen::Matrix<float, 3, 1>& eigen_values, Eigen::Matrix<float, 3, 1>& eigen_vector1, 00147 Eigen::Matrix<float, 3, 1>& eigen_vector2, Eigen::Matrix<float, 3, 1>& eigen_vector3) const 00148 { 00149 //cout << "Using specialized 3x3 version of doPCA!\n"; 00150 Eigen::Matrix<float, 3, 3> eigen_vectors; 00151 eigen33(covariance_, eigen_vectors, eigen_values); 00152 eigen_vector1 = eigen_vectors.col(0); 00153 eigen_vector2 = eigen_vectors.col(1); 00154 eigen_vector3 = eigen_vectors.col(2); 00155 } 00156 template <> 00157 inline void VectorAverage<float, 3>::doPCA(Eigen::Matrix<float, 3, 1>& eigen_values) const 00158 { 00159 //cout << "Using specialized 3x3 version of doPCA!\n"; 00160 computeRoots (covariance_, eigen_values); 00161 } 00162 template <> 00163 inline void VectorAverage<float, 3>::getEigenVector1(Eigen::Matrix<float, 3, 1>& eigen_vector1) const 00164 { 00165 //cout << "Using specialized 3x3 version of doPCA!\n"; 00166 Eigen::Vector3f::Scalar eigen_value; 00167 Eigen::Vector3f eigen_vector; 00168 eigen33(covariance_, eigen_value, eigen_vector); 00169 eigen_vector1 = eigen_vector; 00170 } 00171 00172 //////////// 00173 // double // 00174 //////////// 00175 template <> 00176 inline void VectorAverage<double, 3>::doPCA(Eigen::Matrix<double, 3, 1>& eigen_values, Eigen::Matrix<double, 3, 1>& eigen_vector1, 00177 Eigen::Matrix<double, 3, 1>& eigen_vector2, Eigen::Matrix<double, 3, 1>& eigen_vector3) const 00178 { 00179 //cout << "Using specialized 3x3 version of doPCA!\n"; 00180 Eigen::Matrix<double, 3, 3> eigen_vectors; 00181 eigen33(covariance_, eigen_vectors, eigen_values); 00182 eigen_vector1 = eigen_vectors.col(0); 00183 eigen_vector2 = eigen_vectors.col(1); 00184 eigen_vector3 = eigen_vectors.col(2); 00185 } 00186 template <> 00187 inline void VectorAverage<double, 3>::doPCA(Eigen::Matrix<double, 3, 1>& eigen_values) const 00188 { 00189 //cout << "Using specialized 3x3 version of doPCA!\n"; 00190 computeRoots (covariance_, eigen_values); 00191 } 00192 template <> 00193 inline void VectorAverage<double, 3>::getEigenVector1(Eigen::Matrix<double, 3, 1>& eigen_vector1) const 00194 { 00195 //cout << "Using specialized 3x3 version of doPCA!\n"; 00196 Eigen::Vector3d::Scalar eigen_value; 00197 Eigen::Vector3d eigen_vector; 00198 eigen33(covariance_, eigen_value, eigen_vector); 00199 eigen_vector1 = eigen_vector; 00200 } 00201 } // END namespace 00202 00203 #endif 00204