CovarianceTimeCache.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include <uncertain_tf/CovarianceTimeCache.h>
00004 
00005 using namespace Eigen;
00006 using namespace tf;
00007 using namespace uncertain_tf;
00008 using namespace std;
00009 
00010 CovarianceStorage::CovarianceStorage(const MatrixXd& data, ros::Time stamp)
00011 //, CompactFrameID frame_id)
00012     : covariance_(data)
00013     , stamp_(stamp)
00014     //, frame_id_(frame_id)
00015 { }
00016 
00017 CovarianceTimeCache::CovarianceTimeCache(ros::Duration max_storage_time)
00018     : max_storage_time_(max_storage_time)
00019 {
00020 }
00021 
00022 bool CovarianceTimeCache::getData(ros::Time time, CovarianceStorage & data_out, std::string* error_str)
00023 {
00024     const CovarianceStorage* p_temp_1 = NULL;
00025     const CovarianceStorage* p_temp_2 = NULL;
00026 
00027     int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
00028     if (num_nodes == 0)
00029     {
00030         return false;
00031     }
00032     else if (num_nodes == 1)
00033     {
00034         data_out = *p_temp_1;
00035     }
00036     else if (num_nodes == 2)
00037     {
00038         //if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
00039         //{
00040             interpolate(*p_temp_1, *p_temp_2, time, data_out);
00041         //}
00042         //else
00043         //{
00044         //    data_out = *p_temp_1;
00045         //}
00046     }
00047     else
00048     {
00049 #ifdef ROS_BREAK
00050         ROS_BREAK();
00051 #endif
00052     }
00053 
00054     return true;
00055 }
00056 
00057 bool CovarianceTimeCache::insertData(const CovarianceStorage& new_data)
00058 {
00059     //std::cout << "insertData new_data.stamp_ " << new_data.stamp_ << endl;
00060     if(storage_.begin() != storage_.end())
00061     {
00062         if (storage_.rbegin()->stamp_ > new_data.stamp_ + max_storage_time_)
00063             return false;
00064     }
00065 
00066     storage_.insert(storage_.end(), new_data);
00067 
00068     //std::cout << "storage size " << storage_.size() << endl;
00069 
00070     pruneList();
00071     return true;
00072 }
00073 
00074 
00075 uint8_t CovarianceTimeCache::findClosest(const CovarianceStorage*& one, const CovarianceStorage*& two, ros::Time target_time, std::string* error_str)
00076 {
00077     //No values stored
00078     if (storage_.empty())
00079     {
00080         //createEmptyException(error_str);
00081         return 0;
00082     }
00083 
00084     //If time == 0 return the latest
00085     if (target_time.isZero())
00086     {
00087         one = &(*storage_.rbegin());
00088         return 1;
00089     }
00090 
00091     // One value stored
00092     if (++storage_.begin() == storage_.end())
00093     {
00094         //CovarianceStorage& ts = *storage_.begin();
00095         const CovarianceStorage& ts = *storage_.begin();
00096         if (ts.stamp_ == target_time)
00097         {
00098             one = &ts;
00099             return 1;
00100         }
00101         else
00102         {
00103             //createExtrapolationException1(target_time, ts.stamp_, error_str);
00104             return 0;
00105         }
00106     }
00107 
00108     ros::Time latest_time = (*storage_.rbegin()).stamp_;
00109     ros::Time earliest_time = (*(storage_.begin())).stamp_;
00110 
00111     if (target_time == latest_time)
00112     {
00113         one = &(*storage_.rbegin());
00114         return 1;
00115     }
00116     else if (target_time == earliest_time)
00117     {
00118         one = &(*storage_.begin());
00119         return 1;
00120     }
00121     // Catch cases that would require extrapolation
00122     else if (target_time > latest_time)
00123     {
00124         //createExtrapolationException2(target_time, latest_time, error_str);
00125         ROS_ERROR("EXTRAPOLATION TO FUTURE REQ");
00126         return 0;
00127     }
00128     else if (target_time < earliest_time)
00129     {
00130         //createExtrapolationException3(target_time, earliest_time, error_str);
00131         ROS_ERROR("EXTRAPOLATION TO PAST REQ");
00132         return 0;
00133     }
00134 
00135     //At least 2 values stored
00136     //Find the first value less than the target value
00137     /*L_CovarianceStorage::iterator storage_it = storage_.begin();
00138     while(storage_it != storage_.end())
00139     {
00140         if (storage_it->stamp_ <= target_time)
00141             break;
00142         storage_it++;
00143     }
00144 
00145     //Finally the case were somewhere in the middle  Guarenteed no extrapolation :-)
00146     one = &*(storage_it); //Older
00147     two = &*(--storage_it); //Newer*/
00148 
00149     //Create a temporary object to compare to when searching the lower bound via std::set
00150    CovarianceStorage tmp;
00151    tmp.stamp_ = target_time;
00152 
00153    //Find the first value equal or higher than the target value
00154    L_CovarianceStorage::iterator storage_it = storage_.upper_bound(tmp);
00155 
00156    //Finally the case were somewhere in the middle  Guarenteed no extrapolation :-)
00157    two = &*(storage_it); //Newer
00158    one = &*(--storage_it); //Older
00159 
00160     return 2;
00161 
00162 }
00163 
00164 void CovarianceTimeCache::interpolate(const CovarianceStorage& one, const CovarianceStorage& two, ros::Time time, CovarianceStorage& output)
00165 {
00166     output = two; 
00167 }
00168 
00169 void CovarianceTimeCache::pruneList()
00170 {
00171     ros::Time latest_time = storage_.begin()->stamp_;
00172 
00173     while(!storage_.empty() && storage_.begin()->stamp_ + max_storage_time_ < latest_time)
00174     {
00175         storage_.erase(storage_.begin());
00176     }
00177 }
 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