1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2012, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 */ 37 38 #pragma once 39 40 #include <Eigen/Core> // for Matrix 41 42 #include <pcl/memory.h> 43 #include <pcl/pcl_macros.h> 44 45 namespace pcl 46 { 47 /** \brief Calculates the weighted average and the covariance matrix 48 * 49 * A class to calculate the weighted average and the covariance matrix of a set of vectors with given weights. 50 * The original data is not saved. Mean and covariance are calculated iteratively. 51 * \author Bastian Steder 52 * \ingroup common 53 */ 54 template <typename real, int dimension> 55 class VectorAverage 56 { 57 public: 58 using VectorType = Eigen::Matrix<real, dimension, 1>; 59 using MatrixType = Eigen::Matrix<real, dimension, dimension>; 60 //-----CONSTRUCTOR&DESTRUCTOR----- 61 /** Constructor - dimension gives the size of the vectors to work with. */ 62 VectorAverage (); 63 64 //-----METHODS----- 65 /** Reset the object to work with a new data set */ 66 inline void 67 reset (); 68 69 /** Get the mean of the added vectors */ 70 inline const getMean()71 VectorType& getMean () const { return mean_;} 72 73 /** Get the covariance matrix of the added vectors */ 74 inline const getCovariance()75 MatrixType& getCovariance () const { return covariance_;} 76 77 /** Get the summed up weight of all added vectors */ 78 inline real getAccumulatedWeight()79 getAccumulatedWeight () const { return accumulatedWeight_;} 80 81 /** Get the number of added vectors */ 82 inline unsigned int getNoOfSamples()83 getNoOfSamples () { return noOfSamples_;} 84 85 /** Add a new sample */ 86 inline void 87 add (const VectorType& sample, real weight=1.0); 88 89 /** Do Principal component analysis */ 90 inline void 91 doPCA (VectorType& eigen_values, VectorType& eigen_vector1, 92 VectorType& eigen_vector2, VectorType& eigen_vector3) const; 93 94 /** Do Principal component analysis */ 95 inline void 96 doPCA (VectorType& eigen_values) const; 97 98 /** Get the eigenvector corresponding to the smallest eigenvalue */ 99 inline void 100 getEigenVector1 (VectorType& eigen_vector1) const; 101 102 PCL_MAKE_ALIGNED_OPERATOR_NEW 103 104 //-----VARIABLES----- 105 106 107 protected: 108 //-----METHODS----- 109 //-----VARIABLES----- 110 unsigned int noOfSamples_ = 0; 111 real accumulatedWeight_ = 0; 112 VectorType mean_ = VectorType::Identity (); 113 MatrixType covariance_ = MatrixType::Identity (); 114 }; 115 116 using VectorAverage2f = VectorAverage<float, 2>; 117 using VectorAverage3f = VectorAverage<float, 3>; 118 using VectorAverage4f = VectorAverage<float, 4>; 119 } // END namespace 120 121 #include <pcl/common/impl/vector_average.hpp> 122