gaussianDistribution.h

Go to the documentation of this file.

#ifndef _DLR_GAUSSIANDISTRIBUTION_H_
#define _DLR_GAUSSIANDISTRIBUTION_H_

#include <iostream>
#include <string>
#include <dlrNumeric/array1D.h>
#include <dlrNumeric/array2D.h>

namespace dlr {

  template <class VectorType>
  class GaussianDistribution {
  public:
    /* ******** Public typedefs ******** */
    
    typedef VectorType vector_type;
    
    /* ******** Public member functions ******** */

    GaussianDistribution();

    
     GaussianDistribution(const Array1D<double>& mean,
                          const Array2D<double>& covariance);

    
    GaussianDistribution(const GaussianDistribution<VectorType> &source);

    
    ~GaussianDistribution();

    
    inline void
    addSample(const VectorType& point);

    
    inline size_t
    getCardinality() const {return m_sampleData.size();}

    
    inline const Array2D<double>&
    getCovariance() const;

    
    inline const Array1D<double>&
    getMean() const;

    
    void
    setSampleData(const std::vector<VectorType>& sampleData);

    
    double
    operator()(const VectorType& x) const;

    
    GaussianDistribution<VectorType>&
    operator=(const GaussianDistribution<VectorType>& source);
    
  private:
    /* ********Private member functions******** */

    void
    computeSecondaryStatistics();

    
    void
    computeStatistics();

    
    inline void
    lazyEvaluationFunction() const;

    
    /* ********Private data members******** */

    Array2D<double> m_covariance;
    Array2D<double> m_inverseCovariance;
    Array1D<double> m_mean;
    bool m_mutable;
    bool m_needsUpdate;
    std::vector<VectorType> m_sampleData;
    double m_scalingConstant;
  };

  /* Non-member functions */

  // None
  
}; // namespace dlr


/*================================================================
 * Member function definitions follow.  This would be a .C file
 * if it weren't templated.
 *================================================================*/

#include <cmath>
#include <sstream>
#include <vector>
#include <dlrCommon/exception.h>
#include <dlrNumeric/numericTraits.h>
#include <dlrNumeric/utilities.h>
#include <dlrLinearAlgebra/linearAlgebra.h>

namespace dlr {

  // Default constructor creates an uninitialized distribution.
  template <class VectorType>
  GaussianDistribution<VectorType>::
  GaussianDistribution()
    : m_covariance(),
      m_inverseCovariance(),
      m_mean(),
      m_mutable(true),
      m_needsUpdate(true),
      m_sampleData(),
      m_scalingConstant(0.0)
  {
    // Empty
  }

  // This constructor explicitly sets the mean, and covariance.
  template <class VectorType>
  GaussianDistribution<VectorType>::
  GaussianDistribution(const Array1D<double>& mean,
                       const Array2D<double>& covariance)
    : m_covariance(covariance.copy()),
      m_inverseCovariance(),
      m_mean(mean.copy()),
      m_mutable(false),
      m_needsUpdate(false),
      m_sampleData(),
      m_scalingConstant(0.0)
  {
    this->computeSecondaryStatistics();
  }

  // The copy constructor does a deep copy.
  template <class VectorType>
  GaussianDistribution<VectorType>::
  GaussianDistribution(const GaussianDistribution<VectorType> &source)
    : m_covariance(source.m_covariance.copy()),
      m_inverseCovariance(source.m_inverseCovariance.copy()),
      m_mean(source.m_mean.copy()),
      m_mutable(source.m_mutable),
      m_needsUpdate(source.m_needsUpdate),
      m_sampleData(source.m_sampleData),
      m_scalingConstant(source.m_scalingConstant)

  {
    // Empty
  }

  // The destructor destroys the GaussianDistribution instance and
  // cleans up any allocated storage.
  template <class VectorType>
  GaussianDistribution<VectorType>::
  ~GaussianDistribution()
  {
    // Empty
  }

  // This method adds a sample to the internal list of points from
  // which to compute the distribution statistics.
  template <class VectorType>
  void
  GaussianDistribution<VectorType>::
  addSample(const VectorType& point)
  {
    if(!m_mutable) {
      DLR_THROW3(LogicException,
                 "GaussianDistribution::addSample(const VectorType&)",
                 "Attempt to add sample points when mean and covariance "
                 "have been set explicitly.");
    }
    // Check argument.
    // ... is this the first point?
    if(m_sampleData.size() == 0) {
      // Yes.  Make sure it has a valid size.
      if(point.size() == 0) {
        DLR_THROW3(ValueException,
                   "GaussianDistribution::addSample(...)",
                   "Sample points cannot have size == 0.");
      }
    } else {
      // No.  Make sure it matches the previous points.
      if(point.size() != m_sampleData[0].size()) {
        std::ostringstream message;
        message << "Sample point should have size == "
                << m_sampleData[0].size() << ", but instead has size == "
                << point.size() << ".";
        DLR_THROW3(ValueException,
                   "GaussianDistribution::addSample(...)",
                   message.str().c_str());
      }
    }

    // Done with checks.  Copy the data.
    m_sampleData.push_back(point);

    // And mark *this as dirty.
    m_needsUpdate = true;
  }

  // This member function returns the covariance matrix of the
  // distribution.
  template <class VectorType>
  const Array2D<double>&
  GaussianDistribution<VectorType>::
  getCovariance() const
  {
    // Make sure there aren't any pending operations.
    this->lazyEvaluationFunction();

    // And return the covariance.
    return m_covariance;
  }

  // This member function returns the mean of the distribution.
  template <class VectorType>
  const Array1D<double>&
  GaussianDistribution<VectorType>::
  getMean() const
  {
    // Make sure there aren't any pending operations.
    this->lazyEvaluationFunction();

    // And return the mean.
    return m_mean;
  }

  // This member function replaces the internal list of points from
  // which to compute the distribution statistics.
  template <class VectorType>
  void
  GaussianDistribution<VectorType>::
  setSampleData(const std::vector<VectorType>& sampleData)
  {
    if(!m_mutable) {
      DLR_THROW3(LogicException,
                 "GaussianDistribution::addSample(const VectorType&)",
                 "Attempt to add sample points when mean and covariance "
                 "have been set explicitly.");
    }
    // Check argument.
    // ... Are there any sample points specified?
    if(sampleData.size() != 0) {
      // Yes!  Make sure the first one isn't zero size.
      size_t dimensionality = sampleData[0].size();
      if(dimensionality == 0) {
        DLR_THROW3(ValueException,
                   "GaussianDistribution::setSampleData(...)",
                   "Sample points cannot have size == 0.");
      }
      // And make sure each subsequent point matches the 1st.
      for(size_t index = 0; index < sampleData.size(); ++index) {
        if(sampleData[index].size() != dimensionality) {
          std::ostringstream message;
          message << "Sample point number " << index << " should have size == "
                  << dimensionality << ", but instead has size == "
                  << sampleData[index].size() << ".";
          DLR_THROW3(ValueException,
                     "GaussianDistribution::setSampleData(...)",
                     message.str().c_str());
        }
      }
    }

    // Done with checks.  Copy the data.
    m_sampleData = sampleData;

    // And mark *this as dirty.
    m_needsUpdate = true;
  }
  
  // This member function returns the value of the Gaussian
  // distribution evaluated at the point specified by the argument.
  template <class VectorType>
  double
  GaussianDistribution<VectorType>::
  operator()(const VectorType& x) const
  {
    // First do the stuff we're supposed to have already done...
    this->lazyEvaluationFunction();

    // Check the argument.
    if(x.size() != m_mean.size()) {
      std::ostringstream message;
      message << "Distribution has dimensionality " << m_mean.size() << ", "
              << "but input point has dimensionality " << x.size() << "."
              << std::endl;
      DLR_THROW3(ValueException,
                 "GaussianDistribution::operator()(const VectorType&)",
                 message.str().c_str());
    }

    // First calculate the exponent.
    // ... Actually, we compute -1 * (x - u) here, but it doesn't affect
    // ... the result.
    Array1D<double> normalizedX = m_mean.copy();
    for(size_t index = 0; index < m_mean.size(); ++index) {
      normalizedX -= x[index];
    }
    Array1D<double> inverseCovarianceTimesNormalizedX =
      matrixMultiply(m_inverseCovariance, normalizedX);
    double exponent = dot(normalizedX, inverseCovarianceTimesNormalizedX);

    // Now compute the Gaussian value.
    double returnValue = m_scalingConstant * std::exp(exponent);
    return returnValue;
  }

  // The assignment operator deep copies its argument.
  template <class VectorType>
  GaussianDistribution<VectorType>&
  GaussianDistribution<VectorType>::
  operator=(const GaussianDistribution<VectorType>& source)
  {
    m_covariance = source.m_covariance.copy();
    m_inverseCovariance = source.m_inverseCovariance.copy();
    m_mean = source.m_mean.copy();
    m_mutable = source.m_mutable;
    m_needsUpdate = source.m_needsUpdate;
    m_sampleData = source.m_sampleData;
    m_scalingConstant = source.m_scalingConstant;
    return *this;
  }

  // This private member function computes the inverse covariance
  // matrix and the distribution scaling constant, based on the
  // value of m_covariance.
  template <class VectorType>
  void
  GaussianDistribution<VectorType>::
  computeSecondaryStatistics()
  {
    // We assume that m_covariance has already been set.
    
    // First compute the determinant of the covariance matrix and make
    // sure covariance isn't singular.
    double determinantValue = determinant(m_covariance);
    if(std::fabs(determinantValue) < 1.0E-10) {
      DLR_THROW3(StateException,
                 "GaussianDistribution::computeSecondaryStatistics()",
                 "Computed covariance matrix is singular.  Perhaps "
                 "more sample points are needed?");
    }

    // Use the determinant to compute the scaling constant.
    m_scalingConstant =
                1.0 / std::sqrt(std::pow(static_cast<double>(2.0 * dlr::numeric::constants::pi),
                               static_cast<double>(m_mean.size()))
                      * determinantValue);

    // Next compute inverse covariance.  Should never fail, since
    // determinant is non-zero.
    try {
      m_inverseCovariance = inverse(m_covariance);
    } catch(const ValueException&) {
      DLR_THROW3(RunTimeException,
                 "GaussianDistribution::computeSecondaryStatistics()",
                 "Something is dreadfully wrong, m_covariance has "
                 "non-zero determinant, yet inverse(m_covariance) fails.");
    }
  }
    
  // This private member function estimates the mean, covariance,
  // and any other relevant characteristics of the distribution,
  // based on the provided sample points.
  template <class VectorType>
  void
  GaussianDistribution<VectorType>::
  computeStatistics()
  {
    // First check state.
    if(m_sampleData.size() == 0) {
      DLR_THROW3(StateException,
                 "GaussianDistribution::computeStatistics()",
                 "Can't compute statistics since no sample points have been "
                 "provided.");
    }

    // Now compute the mean value of the sample points.
    // For efficiency, we simultaneously compute covariance.
    // We make use of the identity:
    //   covariance = E[(x - u)*(x - u)^T] = E[x * x^T] - u * u^T
    // where E[.] denotes expected value.
    size_t numberOfSamples = m_sampleData.size();
    size_t dimensionality = m_sampleData[0].size();
    
    m_mean.reinit(dimensionality);
    m_mean = 0.0;
    m_covariance.reinit(dimensionality, dimensionality);
    m_covariance = 0.0;
    for(typename std::vector<VectorType>::iterator
          pointIterator = m_sampleData.begin();
        pointIterator != m_sampleData.end();
        ++pointIterator) {
      for(size_t index0 = 0; index0 < dimensionality; ++index0) {
        m_mean[index0] += (*pointIterator)[index0];
        for(size_t index1 = index0; index1 < dimensionality; ++index1) {
          // Only compute the top half of the covariance matrix for now,
          // since it's symmetric.
          // Note(xxx): Should fix this to run faster.
          m_covariance(index0, index1) +=
            (*pointIterator)[index0] * (*pointIterator)[index1];
        }
      }
    }
    m_mean /= static_cast<double>(numberOfSamples);

    // Warning(xxx): Shouldn't we divide m_covariance by numberOfSamples?
    
    // Now finish up the computation of the covariance matrix.
    for(size_t index0 = 0; index0 < dimensionality; ++index0) {
      for(size_t index1 = index0; index1 < dimensionality; ++index1) {
        // Add correction to the top half of the covariance matrix.
        m_covariance(index0, index1) -=
          numberOfSamples * m_mean[index0] * m_mean[index1];
        // And copy to the bottom half.
        if(index0 != index1) {
          m_covariance(index1, index0) = m_covariance(index0, index1);
        }
      }
    }
    m_covariance /= static_cast<double>(numberOfSamples - 1);
    
    // Finish up by computing anything that depends on these values.
    this->computeSecondaryStatistics();
  }
  
  // This private member function performs any pending calculations.
  template <class VectorType>
  void
  GaussianDistribution<VectorType>::
  lazyEvaluationFunction() const
  {
    if(m_needsUpdate) {
      // This const_cast is one of the penalties of lazy evaluation. :-/
      const_cast<GaussianDistribution<VectorType>*>(this)->computeStatistics();
    }
  }


}; // namespace dlr

#endif // #ifdef _DLR_GAUSSIANDISTRIBUTION_H_

---