Curves
A library for curves generation.
LocalSupport2CoefficientManager.hpp
Go to the documentation of this file.
1 /*
2  * LocalSupport2CoefficientManager.hpp
3  *
4  * Created on: Oct 10, 2014
5  * Author: Paul Furgale, Abel Gawel, Renaud Dube, P├ęter Fankhauser
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
9 #pragma once
10 
11 #include "curves/Curve.hpp"
12 #include <Eigen/Core>
13 #include <boost/unordered_map.hpp>
14 #include <vector>
15 #include <map>
16 
17 namespace curves {
18 
19 typedef size_t Key;
20 
21 template <class Coefficient>
23  public:
25 
26  struct KeyCoefficient {
27  Key key;
28  CoefficientType coefficient;
29 
30  KeyCoefficient(const Key key, const Coefficient& coefficient) :
31  key(key), coefficient(coefficient) {}
32 
34 
35  bool equals(const KeyCoefficient& other) const {
36  //todo Note: here we assume that == operator is implemented by the coefficient.
37  //Could not use gtsam traits as the gtsam namespace is not visible to this class.
38  //Is this correct?
39  return key == other.key && coefficient == other.coefficient;
40  }
41 
42  bool operator==(const KeyCoefficient& other) const {
43  return this->equals(other);
44  }
45  };
46 
47  typedef std::map<Time, KeyCoefficient> TimeToKeyCoefficientMap;
48  typedef typename TimeToKeyCoefficientMap::const_iterator CoefficientIter;
50  typedef boost::unordered_map<size_t, Coefficient> CoefficientMap;
51 
54 
56  bool equals(const LocalSupport2CoefficientManager& other, double tol = 1e-9) const;
57 
59  void print(const std::string& str = "") const;
60 
63  void getKeys(std::vector<Key>* outKeys) const;
64 
67  void appendKeys(std::vector<Key>* outKeys) const;
68 
70  void getTimes(std::vector<Time>* outTimes) const;
71 
73  void getTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const;
74 
76  void modifyCoefficientsValuesInBatch(const std::vector<Time>& times,
77  const std::vector<Coefficient>& values);
78 
79 
85 
90  void insertCoefficients(const std::vector<Time>& times,
91  const std::vector<Coefficient>& values,
92  std::vector<Key>* outKeys = NULL);
93 
95  void addCoefficientAtEnd(Time time, const Coefficient& coefficient, std::vector<Key>* outKeys = NULL);
96 
98  void modifyCoefficient(typename TimeToKeyCoefficientMap::iterator it, Time time, const Coefficient& coefficient);
99 
103  void removeCoefficientWithKey(Key key);
104 
108  void removeCoefficientAtTime(Time time);
109 
111  bool hasCoefficientAtTime(Time time) const;
112 
114  bool hasCoefficientWithKey(Key key) const;
115 
121 
123 
126 
128  Time getCoefficientTimeByKey(Key key) const;
129 
137  bool getCoefficientsAt(Time time, CoefficientIter* outCoefficient0,
138  CoefficientIter* outCoefficient1) const;
139 
141  void getCoefficientsInRange(Time startTime,
142  Time endTime,
143  CoefficientMap* outCoefficients) const;
144 
146  void getCoefficients(CoefficientMap* outCoefficients) const;
147 
151  void updateCoefficients(const CoefficientMap& coefficients);
152 
154  size_t size() const;
155 
157  bool empty() const;
158 
160  void clear();
161 
163  Time getMinTime() const;
164 
166  Time getMaxTime() const;
167 
168  CoefficientIter coefficientBegin() const {
169  return timeToCoefficient_.begin();
170  }
171 
172  CoefficientIter coefficientEnd() const {
173  return timeToCoefficient_.end();
174  }
175 
176  typename TimeToKeyCoefficientMap::iterator coefficientBegin() {
177  return timeToCoefficient_.begin();
178  }
179 
180  typename TimeToKeyCoefficientMap::iterator coefficientEnd() {
181  return timeToCoefficient_.end();
182  }
183 
187  void checkInternalConsistency(bool doExit = false) const;
188 
189  private:
191  boost::unordered_map<Key, CoefficientIter> keyToCoefficient_;
192 
194  TimeToKeyCoefficientMap timeToCoefficient_;
195 
196  bool hasCoefficientAtTime(Time time, CoefficientIter *it, double tol = 0);
197 
198 };
199 
200 } // namespace
201 
Time getMaxTime() const
The one past the last valid time for the curve.
void appendKeys(std::vector< Key > *outKeys) const
bool hasCoefficientAtTime(Time time) const
return true if there is a coefficient at this time
void removeCoefficientWithKey(Key key)
Remove the coefficient with this key.
bool getCoefficientsAt(Time time, CoefficientIter *outCoefficient0, CoefficientIter *outCoefficient1) const
Get the coefficients that are active at a certain time.
void getTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
Get a sorted list of coefficient times in a given time window.
void addCoefficientAtEnd(Time time, const Coefficient &coefficient, std::vector< Key > *outKeys=NULL)
Efficient function for adding a coefficient at the end of the map.
void getKeys(std::vector< Key > *outKeys) const
void removeCoefficientAtTime(Time time)
Remove the coefficient at this time.
Time getCoefficientTimeByKey(Key key) const
get the coefficient time associated with this key
void getCoefficientsInRange(Time startTime, Time endTime, CoefficientMap *outCoefficients) const
Get the coefficients that are active within a range .
Time getMinTime() const
The first valid time for the curve.
Coefficient getCoefficientByKey(Key key) const
set the coefficient associated with this key
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
Key insertCoefficient(Time time, const Coefficient &coefficient)
insert a coefficient at a time and return the key for the coefficient
KeyCoefficient(const Key key, const Coefficient &coefficient)
void getTimes(std::vector< Time > *outTimes) const
Get a sorted list of coefficient times.
void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
void modifyCoefficientsValuesInBatch(const std::vector< Time > &times, const std::vector< Coefficient > &values)
Modify multiple coefficient values. Time is assumed to be ordered.
bool empty() const
Check if the manager is empty.
void updateCoefficientByKey(Key key, const Coefficient &coefficient)
set the coefficient associated with this key
void updateCoefficients(const CoefficientMap &coefficients)
Set coefficients.
double Time
Definition: Curve.hpp:13
bool hasCoefficientWithKey(Key key) const
return true if there is a coefficient with this key
boost::unordered_map< size_t, Coefficient > CoefficientMap
Key/Coefficient pairs.
void getCoefficients(CoefficientMap *outCoefficients) const
Get all of the curve&#39;s coefficients.
TimeToKeyCoefficientMap timeToCoefficient_
Time to coefficient mapping.
void insertCoefficients(const std::vector< Time > &times, const std::vector< Coefficient > &values, std::vector< Key > *outKeys=NULL)
Insert coefficients. Optionally returns the keys for these coefficients.
size_t size() const
return the number of coefficients
boost::unordered_map< Key, CoefficientIter > keyToCoefficient_
Key to coefficient mapping.
TimeToKeyCoefficientMap::const_iterator CoefficientIter
size_t Key
Definition: Curve.hpp:14
TimeToKeyCoefficientMap::iterator coefficientBegin()
void modifyCoefficient(typename TimeToKeyCoefficientMap::iterator it, Time time, const Coefficient &coefficient)
Modify a coefficient by specifying a new time and value.