interpolating_map.h

#pragma once
 
#include <map>
 
#include "core/utils/math/eigen_interface.h"

template <typename KEY, typename VALUE> class InterpolatingMap {
  public:
    using MapType = std::map<KEY, VALUE, std::less<KEY>, Eigen::aligned_allocator<std::pair<const KEY, VALUE>>>;

    void insert(const KEY &key, const VALUE &value) { map_.insert({key, value}); }

    VALUE operator[](const KEY &key) {
        // Get iterator for the upper bound of our key.
        typename MapType::const_iterator upper = map_.upper_bound(key);
 
        // If our key greater than the largest key return its value.
        if (upper == map_.end()) {
            return (--upper)->second;
        }   
 
        // If our key less than or equal to the smallest key return its value.
        if (upper == map_.begin()) {
            return upper->second;
        }
 
        // Get an iterator to the previous entry.
        typename MapType::const_iterator lower = upper;
        --lower;
 
        // Linear interpolate between the values of the first and second.
        const double delta = (key - lower->first) / (upper->first - lower->first);
        return delta * upper->second + (1.0 - delta) * lower->second;
    }

    void clear() { map_.clear(); }
 
  private:
    MapType map_;
};