INTRODUCTION
Overview
Download and Install
Quick Start
Documentation
Publications

NONFRAMEWORK CODE
Driver Interfaces
Drivers
Libraries
Utilities

FRAMEWORK CODE
Interfaces
Components
Libraries
Utilities

Full Software Listings

DEVELOPER
Tutorials
Examples
Dev Guide
Dashboard

PEOPLE
Contributors
Users

SourceForge.net Logo
Project
Download
Mailing lists

 

         

cov3d.h

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

Webmaster: Tobias Kaupp (tobasco at users.sourceforge.net)


Generated for Orca Robotics by  doxygen 1.4.5