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