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