components/laser2og/cov2d.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2009 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 
00011 /***************************************************************************
00012                           cov2d.h  -  description
00013                              -------------------
00014     begin                : Thu Jun 27 2002
00015     copyright            : (C) 2002 by Alex Makarenko
00016     description          : 2D covariance matrix
00017                          : 
00018     versions             : v0 based Tim Baley's code
00019                          : v1 fixed version of ellipse with a,b,th
00020                          : v2 5/1/03 added rotCov()
00021  ***************************************************************************/
00022 
00023 #ifndef QORC_COV2D_H
00024 #define QORC_COV2D_H
00025 
00026 namespace laser2og {
00027 
00028 class Cov2d
00029 {
00030 public:
00031   Cov2d (double xx=0.0, double xy=0.0, double yy=0.0) :
00032                 xx_(xx), xy_(xy), yy_(yy) {}
00033 
00034   double xx() const;
00035   double xy() const;
00036   double yy() const;
00037   void setCov( const double & xx, const double & xy, const double & yy )
00038   { xx_=xx; xy_=xy; yy_=yy; };
00039 
00040   Cov2d inverse() const;
00041   double det() const;
00042   void eigval( double &, double & ) const;
00043   void eigvect( const double &, const double &, double &, double &, double &, double & ) const;
00044 
00045   // rotate ellipse into principle axes
00046   // returns two standard deviations (A,B) in prinicple axes,
00047   // and rotation angle TH from physical CS into principle
00048   void ellipse( double & a, double & b, double & th ) const;
00049 
00050   // similar to above, not maintained
00051   //void ellipse( std::vector<double> &x, std::vector<double> &y, int numPoints ) const;
00052 
00053   // returns 1-D position uncertainty SR in a given direction, e.g. for a single laser return
00054   // requires rotation into principle CS first, so more efficient to do all
00055   // bearing angles BEARING [rad] at once
00056   double rotCov( const double & bearing ) const;
00057   double rotCov( const double & bearing, const double & s1, const double & s2, const double & th ) const;
00058   // similar but handles vectors internally, not maintained
00059   //void rotCov( std::vector<double> & sr, const std::vector<double> & bearing );
00060   
00061 private:
00062         double xx_, xy_, yy_;
00063 };
00064 
00065 
00066 // access
00067 inline double Cov2d::xx() const
00068 { return xx_; }
00069 
00070 inline double Cov2d::xy() const
00071 { return xy_; }
00072 
00073 inline double Cov2d::yy() const
00074 { return yy_; }
00075 
00076 inline double Cov2d::det() const
00077 { return xx_*yy_ - xy_*xy_; }
00078 
00079 
00080 }
00081 #endif