orca-robotics
INTRODUCTION
Overview
Download and Install
Documentation
REPOSITORY
Interfaces
Drivers
Libraries
Utilities
Software Map
DEVELOPER
Dashboard
PEOPLE
Contributors
Users
Project
Download
Mailing lists
hydroqguielementutil/detail/cov2d.h
00001 /* 00002 * Orca-Robotics Project: Components for robotics 00003 * http://orca-robotics.sf.net/ 00004 * Copyright (c) 2004-2008 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 email : a.makarenko@acfr.usyd.edu.au 00017 description : 2D covariance matrix 00018 : 00019 versions : v0 based Tim Baley's code 00020 : v1 fixed version of ellipse with a,b,th 00021 : v2 5/1/03 added rotCov() 00022 ***************************************************************************/ 00023 00024 #ifndef HYDROQGUIPAINT_COV2D_H 00025 #define HYDROQGUIPAINT_COV2D_H 00026 00027 namespace hydroqguielementutil { 00028 00032 class Cov2d 00033 { 00034 public: 00035 Cov2d (double xx=0.0, double xy=0.0, double yy=0.0) : 00036 xx_(xx), xy_(xy), yy_(yy) {} 00037 00038 double xx() const; 00039 double xy() const; 00040 double yy() const; 00041 void setCov( const double & xx, const double & xy, const double & yy ) 00042 { xx_=xx; xy_=xy; yy_=yy; }; 00043 00044 Cov2d inverse() const; 00045 double det() const; 00046 void eigval( double &, double & ) const; 00047 void eigvect( const double &, const double &, double &, double &, double &, double & ) const; 00048 00049 // rotate ellipse into principle axes 00050 // returns two standard deviations (A,B) in prinicple axes, 00051 // and rotation angle TH from physical CS into principle 00052 void ellipse( double & a, double & b, double & th ) const; 00053 00054 // similar to above, not maintained 00055 //void ellipse( std::vector<double> &x, std::vector<double> &y, int numPoints ) const; 00056 00057 // returns 1-D position uncertainty SR in a given direction, e.g. for a single laser return 00058 // requires rotation into principle CS first, so more efficient to do all 00059 // bearing angles BEARING [rad] at once 00060 double rotCov( const double & bearing ) const; 00061 double rotCov( const double & bearing, const double & s1, const double & s2, const double & th ) const; 00062 // similar but handles vectors internally, not maintained 00063 //void rotCov( std::vector<double> & sr, const std::vector<double> & bearing ); 00064 00065 private: 00066 double xx_, xy_, yy_; 00067 }; 00068 00069 00070 // access 00071 inline double Cov2d::Cov2d::xx() const 00072 { return xx_; } 00073 00074 inline double Cov2d::Cov2d::xy() const 00075 { return xy_; } 00076 00077 inline double Cov2d::Cov2d::yy() const 00078 { return yy_; } 00079 00080 inline double Cov2d::det() const 00081 { return xx_*yy_ - xy_*xy_; } 00082 00083 00084 } 00085 #endif
