Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks
00005  *
00006  * This distribution is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef HYDRONAVUTIL_GMM_H
00011 #define HYDRONAVUTIL_GMM_H
00012 
00013 #include <vector>
00014 #include <hydronavutil/cov3d.h>
00015 #include <hydronavutil/pose.h>
00016 #include <assert.h>
00017 
00018 namespace hydronavutil {
00019 
00025 class Gaussian
00026 {
00027 
00028 public:
00029 
00030     Gaussian() {}
00031     Gaussian( const Pose &mean, const Cov3d &cov )
00032         : mean_(mean), cov_(cov)
00033         {}
00034 
00035     // non-const versions
00036     Pose   &mean() { return mean_; }
00037     Cov3d  &cov()  { return cov_; }
00038 
00039     // const versions
00040     const Pose &mean() const { return mean_; }
00041     const Cov3d &cov() const { return cov_; }
00042 
00043 private:
00044 
00045     // The mean pose
00046     Pose  mean_;
00047 
00048     // The covariance of the gaussian (x,y,theta)
00049     Cov3d cov_;
00050 
00051     // The weight of this component in the mixture
00052     double weight_;
00053 };
00054 std::ostream &operator<<( std::ostream &s, const Gaussian &g );
00055 
00061 class Gmm
00062 {
00063 
00064 public: 
00065 
00066     Gmm() {}
00067 
00069     Gmm( const Pose &mean, const Cov3d &cov );
00070 
00072     Gmm( const std::vector<Gaussian> &components,
00073          const std::vector<double>   &weights );
00074 
00075     unsigned int size() const { return components_.size(); }
00076 
00078     const Gaussian &components(int i) const
00079         {
00080             assert( i < (int)(components_.size()) );
00081             return components_[i];
00082         }
00084     double weights(int i) const
00085         {
00086             assert( i < (int)(weights_.size()) );
00087             return weights_[i];
00088         }
00089 
00091     Gaussian &components(int i)
00092         {
00093             assert( i < (int)(components_.size()) );
00094             return components_[i];
00095         }
00097     double &weights(int i)
00098         {
00099             assert( i < (int)(weights_.size()) );
00100             return weights_[i];
00101         }
00102 
00104     int mlComponentI() const;
00105 
00107     const Gaussian &mlComponent() const;
00108 
00110     void addComponent( const Gaussian &comp, double weight );
00111 
00112 private: 
00113 
00114     // Makes the weights sum to 1.0
00115     void normaliseWeights();
00116 
00117     // throws exceptions if not sane
00118     void checkIsSane();
00119 
00120     std::vector<Gaussian> components_;
00121     std::vector<double>   weights_;
00122 };
00123 std::ostream &operator<<( std::ostream &s, const Gmm &g );
00124 
00127 bool isUncertain( const Gmm &gmm,
00128                   double maxLinearVar     = 5.0,
00129                   double maxRotationalVar = (M_PI/2.0),
00130                   int    maxComponents    = 2 );
00131 
00132 }
00133 
00134 #endif
gmm.h
