Point Cloud Library (PCL)  1.3.1
vector_average.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines