msf_sortedContainer.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
 * You can contact the author at <slynen at ethz dot ch>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#ifndef MSF_SORTEDCONTAINER_H_
#define MSF_SORTEDCONTAINER_H_

#include <msf_core/msf_types.hpp>
#include <msf_core/msf_tools.h>
#include <msf_core/msf_macros.h>
#include <iomanip>

#define CHECK_IN_BOUNDS(iterator, container) \
           do { \
             decltype(iterator) __it = it; \
             ++__it; \
             if (__it == container.begin()) { \
               MSF_ERROR_STREAM("Iterator out of bounds (begin) " << \
                 __FILE__ << ":" << __LINE__); \
             } \
             if (iterator == container.end()) { \
               MSF_ERROR_STREAM("Iterator out of bounds (end) " << \
                 __FILE__ << ":" << __LINE__); \
             } \
           } while(0);


namespace msf_core {
template<typename T, typename PrototypeInvalidT = T>
class SortedContainer {

 public:
  typedef shared_ptr<T> Ptr_T;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  typedef std::map<double, Ptr_T> ListT;  
  ListT stateList;  
  Ptr_T invalid;  
 public:
  typedef typename ListT::iterator iterator_T;

  SortedContainer() {
    invalid.reset(new PrototypeInvalidT());
    invalid->time = -1;
  }
  inline shared_ptr<T>& getInvalid() {
    return invalid;
  }

  inline void clear() {
    stateList.clear();
  }

  inline typename ListT::size_type size() {
    return stateList.size();
  }

  inline typename ListT::iterator insert(const shared_ptr<T>& value) {
    std::pair<typename ListT::iterator, bool> itpr = stateList.insert(
        std::pair<double, shared_ptr<T> >(value->time, value));
    if (!itpr.second) {
      MSF_WARN_STREAM(
          "Wanted to insert a value to the sorted container at time " <<
          std::fixed << std::setprecision(9) << value->time <<
          " but the map already contained a value at this time. discarding.");
    }
    return itpr.first;
  }

  inline typename ListT::iterator getIteratorBegin() {
    return stateList.begin();
  }

  inline typename ListT::iterator getIteratorBeforeBegin() {
    typename ListT::iterator it = stateList.begin();
    return --it;
  }

  inline typename ListT::iterator getIteratorEnd() {
    return stateList.end();
  }

  inline typename ListT::iterator getIteratorAtValue(const shared_ptr<T>& value,
                                                     bool warnIfNotExistant =
                                                         true) {
    typename ListT::iterator it = stateList.find(value->time);
    if (it == stateList.end()) {  // There is no value in the map with this time.
      if (warnIfNotExistant)
        MSF_WARN_STREAM(
            "getIteratorAtValue(state): Could not find value for time " <<
            std::fixed << std::setprecision(9) << value->time);
      it = stateList.lower_bound(value->time);
    }
    return it;
  }

  inline typename ListT::iterator getIteratorAtValue(const double& time,
                                                     bool warnIfNotExistant =
                                                         true) {
    typename ListT::iterator it = stateList.find(time);
    if (it == stateList.end()) {  //there is no value in the map with this time
      if (warnIfNotExistant)
        MSF_WARN_STREAM(
            "getIteratorAtValue(double): Could not find value for time " <<
            std::fixed << std::setprecision(9) << time);
      it = stateList.lower_bound(time);
    }
    return it;
  }

  inline typename ListT::iterator getIteratorClosestBefore(
      const double& statetime) {
    typename ListT::iterator it = stateList.lower_bound(statetime);
    it--;
    return it;
  }

  inline typename ListT::iterator getIteratorClosestAfter(
      const double& statetime) {
    typename ListT::iterator it = stateList.upper_bound(statetime);
    return it;
  }

  inline typename ListT::iterator getIteratorClosest(const double& statetime) {

    // First check if we have a value at this time in the buffer.
    typename ListT::iterator it_at = stateList.find(statetime);
    if (it_at != stateList.end()) {
      return it_at;
    }

    // If not, find the closest one.
    typename ListT::iterator tauMinus = getIteratorClosestBefore(statetime);
    typename ListT::iterator tauPlus = getIteratorClosestAfter(statetime);

    typename ListT::iterator it_end = getIteratorEnd();
    if (tauMinus == it_end) {
      return tauPlus;
    }
    if (tauPlus == it_end) {
      return tauMinus;
    }
    if (fabs(tauPlus->second->time - statetime)
        < fabs(tauMinus->second->time - statetime)) {
      return tauPlus;
    } else {
      return tauMinus;
    }
  }

  inline shared_ptr<T>& getClosestBefore(const double& statetime) {
    typename ListT::iterator it = stateList.lower_bound(statetime);
    if (stateList.empty()) {
      MSF_WARN_STREAM("Requested the first object before time " << statetime <<
        "but the container is empty");
      return getInvalid();
    }
    if (it == stateList.begin()) {
      return it->second;
    }
    it--;
    return it->second;
  }
  ;

  inline shared_ptr<T>& getClosestAfter(const double& statetime) {
    typename ListT::iterator it = stateList.upper_bound(statetime);
    if (it == stateList.end()) {
      return getInvalid();
    }
    return it->second;
  }
  ;

  inline shared_ptr<T>& getValueAt(const double& statetime) {
    typename ListT::iterator it = stateList.find(statetime);
    if (it == stateList.end()) {
      return getInvalid();
    }
    return it->second;
  }
  ;

  inline shared_ptr<T>& getClosest(const double& statetime) {
    shared_ptr<T>& at = getValueAt(statetime);  // Is there one exactly at this point?
    if (at != getInvalid()) {
      return at;
    }

    shared_ptr<T>& tauMinus = getClosestBefore(statetime);
    shared_ptr<T>& tauPlus = getClosestAfter(statetime);

    if (tauMinus->time == -1) {
      return tauPlus;
    } else if (tauPlus->time == -1) {
      return tauMinus;
    }

    if (fabs(tauPlus->time - statetime) < fabs(tauMinus->time - statetime)) {
      return tauPlus;
    } else {
      return tauMinus;
    }
  }

  inline void clearOlderThan(double age) {
    double newest = getLast()->time;
    iterator_T it = getIteratorClosest(newest - age);
    if (newest - it->second->time < age)
      return;  //there is no state older than time
    if (it->second->time > stateList.begin()->second->time)
      stateList.erase(stateList.begin(), it);
  }

  inline shared_ptr<T>& getLast() {
    if (stateList.empty()) {
      MSF_WARN_STREAM("Requested the last object in the sorted container, but "
      "the container is empty");
      return getInvalid();
    }
    typename ListT::iterator end = stateList.end();
    return (--end)->second;
  }

  inline shared_ptr<T>& getFirst() {
    if (stateList.empty()) {
      MSF_WARN_STREAM("Requested the first object in the sorted container, "
      "but the container is empty");
      return getInvalid();
    }
    typename ListT::iterator start = stateList.begin();
    return start->second;
  }

  inline shared_ptr<T> updateTime(double timeOld, double timeNew)
      __attribute__ ((warn_unused_result)) {
    typename ListT::iterator it = stateList.find(timeOld);
    if (it == stateList.end()) {
      std::stringstream ss;
      ss
          << "Wanted to update a states/measurements time, but could not find "
              "the old state, for which the time was asked to be updated. time "
          << std::fixed << std::setprecision(9) << timeOld << std::endl;

      ss << "Map: " << std::endl;
      for (typename ListT::iterator it2 = stateList.begin();
          it2 != stateList.end(); ++it2) {
        ss << it2->first << std::endl;
      }
      MSF_WARN_STREAM(ss.str());

      return getClosest(timeOld);
    }
    // Get the data from the map, we need to update, then reinsert.
    shared_ptr<T> copy = it->second;
    stateList.erase(it->first);
    copy->time = timeNew;
    typename ListT::iterator inserted = insert(copy);
    return inserted->second;
  }

  std::string echoBufferContentTimes() {
    std::stringstream ss;

    for (typename ListT::iterator it = getIteratorBegin();
        it != getIteratorEnd(); ++it) {
      ss << std::fixed << std::setprecision(9) << it->second->time << std::endl;
    }
    return ss.str();

  }

};
}

#endif  // MSF_SORTEDCONTAINER_H_