CovarianceTimeCache.h
Go to the documentation of this file.
00001 #ifndef _COVARIANCE_TIME_CACHE_H_
00002 #define _COVARIANCE_TIME_CACHE_H_
00003 
00004 #include "tf/tf.h"
00005 #include <Eigen/Dense>
00006 #include <set>
00007 
00008 using Eigen::MatrixXd;
00009 
00010 namespace uncertain_tf
00011 {
00012 
00013 typedef tf::Stamped<Eigen::MatrixXd> StampedCovariance;
00014 
00015 class CovarianceStorage
00016 {
00017 public :
00018 
00019     CovarianceStorage() {};
00020 
00021     CovarianceStorage(const MatrixXd& data, ros::Time stamp); //, CompactFrameID frame_id);
00022 
00023     CovarianceStorage(const CovarianceStorage& rhs)
00024     {
00025         *this = rhs;
00026     }
00027 
00028     CovarianceStorage& operator=(const CovarianceStorage& rhs)
00029     {
00030        covariance_ = rhs.covariance_;
00031         stamp_ = rhs.stamp_;
00032         //frame_id_ = rhs.frame_id_;
00033         return *this;
00034     }
00035 
00036     bool operator< (const CovarianceStorage &b) const
00037     {
00038       return this->stamp_ < b.stamp_;
00039     }
00040 
00041 
00042     MatrixXd covariance_;
00043     ros::Time stamp_;
00044 
00045     //CompactFrameID frame_id_; // we do not store frame_ids since we always assume the connectivity at a given time is stored in the tf tree, and the covariances fit
00046 };
00047 
00048 
00049 class CovarianceTimeCache
00050 {
00051 public:
00052     static const int MIN_INTERPOLATION_DISTANCE = 5; 
00053     static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; 
00054     static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL; 
00055 
00056     CovarianceTimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME));
00057 
00058     bool getData(ros::Time time, CovarianceStorage &data_out, std::string* error_str = 0);
00059 
00060     bool insertData(const CovarianceStorage& new_data);
00061 
00062     inline uint8_t findClosest(const CovarianceStorage*& one,const CovarianceStorage*& two, ros::Time target_time, std::string* error_str);
00063 
00064     inline void interpolate(const CovarianceStorage& one, const CovarianceStorage& two, ros::Time time, CovarianceStorage& output);
00065 
00066     void pruneList();
00067 
00068     typedef std::set<CovarianceStorage> L_CovarianceStorage;
00069     L_CovarianceStorage storage_;
00070 
00071     ros::Duration max_storage_time_;
00072 
00073 
00074 };
00075 
00076 }
00077 
00078 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


uncertain_tf
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 17:49:36