Orca Robotics

00001 #ifndef ORCANAVUTIL_COV3D_H
00002 #define ORCANAVUTIL_COV3D_H
00003 
00004 #include <iostream>
00005 #include <assert.h>
00006 #include <hydronavutil/exceptions.h>
00007 
00008 namespace hydronavutil {
00009 
00015 class Cov3d
00016 {
00017 private: 
00018 
00019     static const int PXX   = 0;
00020     static const int PXY   = 1;
00021     static const int PXT   = 2;
00022 
00023     static const int PYY   = 3;
00024     static const int PYT   = 4;
00025     static const int PTT   = 5;
00026 
00027 public: 
00028 
00029     Cov3d() {}
00030 
00032     Cov3d( double xx, 
00033            double xy, 
00034            double xt,
00035            double yy,
00036            double yt,
00037            double tt )
00038         {
00039             mat_[PXX] = xx;
00040             mat_[PXY] = xy;
00041             mat_[PXT] = xt;
00042             mat_[PYY] = yy;
00043             mat_[PYT] = yt;
00044             mat_[PTT] = tt;
00045         }
00046 
00048     Cov3d( double xx, 
00049            double yy,
00050            double tt )
00051         {
00052             mat_[PXX] = xx;
00053             mat_[PXY] = 0;
00054             mat_[PXT] = 0;
00055             mat_[PYY] = yy;
00056             mat_[PYT] = 0;
00057             mat_[PTT] = tt;
00058         }
00059 
00060     double &xx()   { return mat_[PXX]; }
00061     double &xy()   { return mat_[PXY]; }
00062     double &xt()   { return mat_[PXT]; }
00063     double &yy()   { return mat_[PYY]; }
00064     double &yt()   { return mat_[PYT]; }
00065     double &tt()   { return mat_[PTT]; }
00066 
00067     double xx() const   { return mat_[PXX]; }
00068     double xy() const   { return mat_[PXY]; }
00069     double xt() const   { return mat_[PXT]; }
00070     double yy() const   { return mat_[PYY]; }
00071     double yt() const   { return mat_[PYT]; }
00072     double tt() const   { return mat_[PTT]; }
00073 
00075     double det() const;
00076     Cov3d inverse() const;
00077 
00080     double gauss( double x, double y, double t ) const;
00081 
00083     double m( int i, int j ) const
00084         {
00085             if      ( i==0 && j==0 ) return xx();
00086             else if ( i==0 && j==1 || i==1 && j==0 ) return xy();
00087             else if ( i==0 && j==2 || i==2 && j==0 ) return xt();
00088             else if ( i==1 && j==1 ) return yy();
00089             else if ( i==1 && j==2 || i==2 && j==1 ) return yt();
00090             else if ( i==2 && j==2 ) return tt();
00091             else { assert( false&&"bad index" ); return 0; }
00092         }
00094     double &m( int i, int j )
00095         {
00096             if      ( i==0 && j==0 ) return xx();
00097             else if ( i==0 && j==1 || i==1 && j==0 ) return xy();
00098             else if ( i==0 && j==2 || i==2 && j==0 ) return xt();
00099             else if ( i==1 && j==1 ) return yy();
00100             else if ( i==1 && j==2 || i==2 && j==1 ) return yt();
00101             else if ( i==2 && j==2 ) return tt();
00102             else { assert( false&&"bad index" ); exit(1); }
00103         }
00104 
00105     bool isSPD() const
00106         { return det() > 0; }
00107 
00108 private: 
00109 
00110     double mat_[6];
00111 };
00112 
00113 std::ostream &operator<<( std::ostream &s, const Cov3d &c );
00114 
00115 // Allow use outside of class
00116 inline double det( double xx,
00117                    double xy,
00118                    double xt,
00119                    double yy,
00120                    double yt,
00121                    double tt )
00122 {
00123     double ytSq = yt*yt;
00124     double xySq = xy*xy;
00125     double xtSq = xt*xt;
00126     return xx*yy*tt - xx*ytSq - xySq*tt + 2*xy*xt*yt - xtSq*yy;    
00127 }         
00128 
00129 // Allow use outside of class
00130 inline void invert( double xx,
00131                     double xy,
00132                     double xt,
00133                     double yy,
00134                     double yt,
00135                     double tt,
00136                     double &xxOut,
00137                     double &xyOut,
00138                     double &xtOut,
00139                     double &yyOut,
00140                     double &ytOut,
00141                     double &ttOut )
00142 {
00143     double d = det( xx, xy, xt, yy, yt, tt );
00144 
00145     if ( d == 0 )
00146         throw hydronavutil::Exception( ERROR_INFO, "found zero determinant during attempt to invert" );
00147 
00148     double ytSq = yt*yt;
00149     double xySq = xy*xy;
00150     double xtSq = xt*xt;
00151 
00152     xxOut = (yy*tt-ytSq)/d;
00153     xyOut = -(xy*tt-xt*yt)/d;
00154     xtOut = (xy*yt-xt*yy)/d;
00155     yyOut = (xx*tt-xtSq)/d;
00156     ytOut = -(xx*yt-xy*xt)/d;
00157     ttOut = (xx*yy-xySq)/d;
00158 }
00159 
00160 // Allow use outside of class.
00163 inline double gauss( double xx,
00164                      double xy,
00165                      double xt,
00166                      double yy,
00167                      double yt,
00168                      double tt,
00169                      double x,
00170                      double y,
00171                      double t )
00172 {
00173     Cov3d c( xx, xy, xt, yy, yt, tt );
00174     return c.gauss( x, y, t );
00175 }
00176 
00177 }
00178 
00179 #endif
cov3d.h
