Curves
A library for curves generation.
LocalSupport2CoefficientManager-inl.hpp
Go to the documentation of this file.
1 /*
2  * LocalSupport2CoefficientManager-inl.hpp
3  *
4  * Created on: Aug 17, 2014
5  * Author: Paul Furgale, Abel Gawel, Renaud Dube, P├ęter Fankhauser
6  * Institute: ETH Zurich, Autonomous Systems Lab
7  */
8 
10 
11 #include <iostream>
13 #include <curves/KeyGenerator.hpp>
14 #include <glog/logging.h>
15 
16 namespace curves {
17 
18 template <class Coefficient>
20 }
21 
22 template <class Coefficient>
24 }
25 
27 template <class Coefficient>
29  double tol) const {
30  bool equal = true;
31  equal &= keyToCoefficient_.size() == other.keyToCoefficient_.size();
32  equal &= timeToCoefficient_.size() == other.timeToCoefficient_.size();
33  if (equal) {
34  CoefficientIter it1, it2;
35  it1 = keyToCoefficient_.begin();
36  it2 = other.keyToCoefficient_.begin();
37  for( ; it1 != keyToCoefficient_.end(); ++it1, ++it2) {
38  equal &= it1->first == it2->first;
39  equal &= it1->second.equals(it2->second);
40  }
41 
42  CoefficientIter it3, it4;
43  it3 = timeToCoefficient_.begin();
44  it4 = other.timeToCoefficient_.begin();
45  for( ; it3 != timeToCoefficient_.end(); ++it3, ++it4) {
46  equal &= it3->first == it4->first;
47  if (it3->second && it4->second) {
48  equal &= it3->second->equals(*(it4->second));
49  } else {
50  equal &= it3->second == it4->second;
51  }
52  }
53 
54  }
55  return equal;
56 }
57 
58 template <class Coefficient>
59 void LocalSupport2CoefficientManager<Coefficient>::getKeys(std::vector<Key>* outKeys) const {
60  CHECK_NOTNULL(outKeys);
61  outKeys->clear();
62  appendKeys(outKeys);
63 }
64 
65 template <class Coefficient>
66 void LocalSupport2CoefficientManager<Coefficient>::appendKeys(std::vector<Key>* outKeys) const {
67  CHECK_NOTNULL(outKeys);
68  outKeys->reserve(outKeys->size() + keyToCoefficient_.size());
69  CoefficientIter it;
70  it = timeToCoefficient_.begin();
71  for( ; it != timeToCoefficient_.end(); ++it) {
72  outKeys->push_back(it->second.key);
73  }
74 }
75 
76 template <class Coefficient>
77 void LocalSupport2CoefficientManager<Coefficient>::getTimes(std::vector<Time>* outTimes) const {
78  CHECK_NOTNULL(outTimes);
79  outTimes->clear();
80  outTimes->reserve(timeToCoefficient_.size());
81  CoefficientIter it;
82  it = timeToCoefficient_.begin();
83  for( ; it != timeToCoefficient_.end(); ++it) {
84  outTimes->push_back(it->first);
85  }
86 }
87 
88 template <class Coefficient>
90  Time begTime, Time endTime) const {
91  CHECK_EQ(endTime, getMaxTime()) << "Not implemented for window not at the end.";
92  CHECK(begTime >= getMinTime()) << "Asked for times outside the curve.";
93  CHECK_NOTNULL(outTimes);
94 
95  outTimes->clear();
96  CoefficientIter it = --(timeToCoefficient_.end());
97 
98  do {
99  if (it->first >= begTime) {
100  outTimes->push_back(it->first);
101  }
102  --it;
103  } while (it->first >= begTime && it != timeToCoefficient_.begin());
104 
105  std::reverse(outTimes->begin(),outTimes->end());
106 }
107 
108 template <class Coefficient>
109 void LocalSupport2CoefficientManager<Coefficient>::print(const std::string& str) const {
110  // \todo (Abel or Renaud)
111 }
112 
113 template <class Coefficient>
115  CoefficientIter it;
116  Key key;
117 
118  if (this->hasCoefficientAtTime(time, &it)) {
119  this->updateCoefficientByKey(it->second.key, coefficient);
120  key = it->second.key;
121  } else {
122  key = KeyGenerator::getNextKey();
123  std::pair<Time, KeyCoefficient> iterator(time, KeyCoefficient(key, coefficient));
124  std::pair<CoefficientIter, bool> success =
125  timeToCoefficient_.insert(iterator);
126  keyToCoefficient_[key] = success.first;
127  }
128  return key;
129 }
130 
132 template <class Coefficient>
134  const std::vector<Coefficient>& values,
135  std::vector<Key>* outKeys) {
136  CHECK_EQ(times.size(), values.size());
137  for(Key i = 0; i < times.size(); ++i) {
138  if (outKeys != NULL) {
139  outKeys->push_back(insertCoefficient(times[i], values[i]));
140  } else {
141  insertCoefficient(times[i], values[i]);
142  }
143  }
144 }
145 
146 template <class Coefficient>
148  const std::vector<Coefficient>& values) {
149  CHECK_EQ(times.size(), values.size());
150  // Get an iterator to the first coefficient
151  typename TimeToKeyCoefficientMap::iterator it = timeToCoefficient_.end();
152 
153  do {
154  --it;
155  } while (it->first != times[0]);
156 
157  for (size_t i = 0; i < times.size(); ++i) {
158  CHECK_EQ(it->first,times[i]);
159  it->second.coefficient = values[i];
160  ++it;
161  }
162 }
163 
164 template <class Coefficient>
165 void LocalSupport2CoefficientManager<Coefficient>::addCoefficientAtEnd(Time time, const Coefficient& coefficient, std::vector<Key>* outKeys) {
166  CHECK(time > getMaxTime()) << "Time to add is not greater than curve max time";
167 
169 
170  // Insert the coefficient with a hint that it goes at the end
171  CoefficientIter it = timeToCoefficient_.insert(--(timeToCoefficient_.end()),
172  std::pair<Time, KeyCoefficient>(time, KeyCoefficient(key, coefficient)));
173 
174  keyToCoefficient_.insert(keyToCoefficient_.end(), std::pair<Key, CoefficientIter>(key,it));
175  if (outKeys != NULL) {
176  outKeys->push_back(key);
177  }
178 }
179 
180 template <class Coefficient>
181 void LocalSupport2CoefficientManager<Coefficient>::modifyCoefficient(typename TimeToKeyCoefficientMap::iterator it,
182  Time time, const Coefficient& coefficient) {
183  // This is used by slerp sampling policy.
184  // In this case a new coefficient should be placed slightly later than the initial one.
185  CoefficientIter newIt = timeToCoefficient_.insert(it,std::pair<Time, KeyCoefficient>(time, KeyCoefficient(it->second.key, coefficient)));
186  // Update keyToCoefficient_
187  keyToCoefficient_[it->second.key] = newIt;
188  // Remove the old coefficient
189  timeToCoefficient_.erase(it);
190 }
191 
192 template <class Coefficient>
194  CHECK(hasCoefficientWithKey(key)) << "No coefficient with that key.";
195  typename TimeToKeyCoefficientMap::iterator it1;
196  typename boost::unordered_map<Key, CoefficientIter>::iterator it2;
197  it2 = keyToCoefficient_.find(key);
198  it1 = timeToCoefficient_.find(it1->second->first);
199  timeToCoefficient_.erase(it1);
200  keyToCoefficient_.erase(it2);
201 }
202 
203 template <class Coefficient>
205  CHECK(this->hasCoefficientAtTime(time)) << "No coefficient at that time.";
206  typename TimeToKeyCoefficientMap::iterator it1;
207  typename boost::unordered_map<Key, CoefficientIter>::iterator it2;
208  it1 = timeToCoefficient_.find(time);
209  it2 = keyToCoefficient_.find(it1->second.key);
210  timeToCoefficient_.erase(it1);
211  keyToCoefficient_.erase(it2);
212 }
213 
215 template <class Coefficient>
217  CoefficientIter it = timeToCoefficient_.find(time);
218  return it != timeToCoefficient_.end();
219 }
220 
222 template <class Coefficient>
224  CoefficientIter it = keyToCoefficient_.find(key);
225  return it != keyToCoefficient_.end();
226 }
227 
232 template <class Coefficient>
234  typename boost::unordered_map<Key, CoefficientIter>::iterator it = keyToCoefficient_.find(key);
235  CHECK(it != keyToCoefficient_.end()) << "Key " << key << " is not in the container.";
236  *const_cast<CoefficientType*>(&(it)->second->second.coefficient) = coefficient;
237 }
238 
240 template <class Coefficient>
242  typename boost::unordered_map<Key, CoefficientIter>::const_iterator it = keyToCoefficient_.find(key);
243  CHECK(it != keyToCoefficient_.end() ) << "Key " << key << " is not in the container.";
244  return it->second->second.coefficient;
245 }
246 template <class Coefficient>
248  typename boost::unordered_map<Key, CoefficientIter>::const_iterator it = keyToCoefficient_.find(key);
249  CHECK(it != keyToCoefficient_.end()) << "Key " << key << " is not in the container.";
250  return it->second->first;
251 }
252 
253 
255 template <class Coefficient>
257  CoefficientIter* outCoefficient0,
258  CoefficientIter* outCoefficient1) const {
259  CHECK_NOTNULL(outCoefficient0);
260  CHECK_NOTNULL(outCoefficient1);
261  if( timeToCoefficient_.empty() ) {
262  LOG(INFO) << "No coefficients";
263  return false;
264  }
265 
266  CoefficientIter it;
267 
268  if(time == getMaxTime()) {
269  it = timeToCoefficient_.end();
270  --it;
271  } else {
272  it = timeToCoefficient_.upper_bound(time);
273  }
274  if(it == timeToCoefficient_.begin() || it == timeToCoefficient_.end()) {
275  LOG(INFO) << "time, " << time << ", is out of bounds: [" << getMinTime() << ", " << getMaxTime() << "]";
276  return false;
277  }
278  --it;
279 
280  // Okay. Now we know that the time is bracketed by
281  // it and it + 1.
282  *outCoefficient0 = it;
283  *outCoefficient1 = (++it);
284 
285  return true;
286 }
287 
289 template <class Coefficient>
291  Time startTime, Time endTime, CoefficientMap* outCoefficients) const {
292 
293  if (startTime <= endTime && startTime <= this->getMaxTime()
294  && endTime >= this->getMinTime()) {
295  // be forgiving if start time is lower than definition of curve
296  if (startTime < this->getMinTime()) {
297  startTime = this->getMinTime();
298  }
299  // be forgiving if end time is greater than definition of curve
300  if (endTime > this->getMaxTime()) {
301  endTime = this->getMaxTime();
302  }
303  CoefficientIter it;
304  // set iterator to coefficient left or equal of start time
305  it = timeToCoefficient_.upper_bound(startTime);
306  it--;
307  // iterate through coefficients
308  for (; it != timeToCoefficient_.end() && it->first < endTime; ++it) {
309  (*outCoefficients)[it->second->key] = it->second->coefficient;
310  }
311  if (it != timeToCoefficient_.end()) {
312  (*outCoefficients)[it->second->key] = it->second->coefficient;
313  }
314  }
315 }
316 
318 template <class Coefficient>
320  CHECK_NOTNULL(outCoefficients);
321  CoefficientIter it;
322  it = timeToCoefficient_.begin();
323  for( ; it != timeToCoefficient_.end(); ++it) {
324  (*outCoefficients)[it->second->key] = it->second->coefficient;
325  }
326 }
327 
331 template <class Coefficient>
333  const CoefficientMap& coefficients) {
334  typename CoefficientMap::const_iterator it;
335  it = coefficients.cbegin();
336  for (; it != coefficients.end(); ++it) {
337  this->updateCoefficientByKey(it->first, it->second);
338  }
339 }
340 
342 template <class Coefficient>
344  return timeToCoefficient_.size();
345 }
346 
347 template <class Coefficient>
349  return timeToCoefficient_.empty();
350 }
351 
353 template <class Coefficient>
355  keyToCoefficient_.clear();
356  timeToCoefficient_.clear();
357 }
358 
359 template <class Coefficient>
361  if (timeToCoefficient_.empty()) {
362  return 0;
363  }
364  return timeToCoefficient_.begin()->first;
365 }
366 
367 template <class Coefficient>
369  if (timeToCoefficient_.empty()) {
370  return 0;
371  }
372  return timeToCoefficient_.rbegin()->first;
373 }
374 
375 template <class Coefficient>
377  CHECK_EQ(keyToCoefficient_.size(), timeToCoefficient_.size());
378  CoefficientIter it;
379  typename boost::unordered_map<Key, CoefficientIter>::const_iterator itc;
380  for(it = timeToCoefficient_.begin() ; it != timeToCoefficient_.end(); ++it) {
381  Key key = it->second.key;
382  itc = keyToCoefficient_.find(key);
383  CHECK_EQ(itc->first, itc->second->second.key);
384  CHECK( itc != keyToCoefficient_.end() ) << "Key " << key << " is not in the map";
385  // This is probably the important one.
386  // Check that the itc->second iterator
387  // points to the same object as it->second.
388  // It is supposedly guaranteed by the
389  // unordered map interface that these
390  // pointers never get reallocated.
391  //todo the implementation differs from the comment above. Here only the
392  //equality between coefficients is checked
393  CHECK_EQ(itc->second->second.coefficient, it->second.coefficient);
394  }
395  if (doExit) {
396  exit(0);
397  }
398 }
399 
400 template <class Coefficient>
402  for ((*it) = timeToCoefficient_.begin();
403  (*it) != timeToCoefficient_.end(); ++(*it)) {
404  if ((*it)->first >= time-tol) {
405  if ((*it)->first <= time+tol) {
406  return true;
407  } else {
408  return false;
409  }
410  }
411  }
412  return false;
413 }
414 
415 } // namespace
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 .
static size_t getNextKey()
Time getMinTime() const
The first valid time for the curve.
Coefficient getCoefficientByKey(Key key) const
set the coefficient associated with this key
bool equals(const LocalSupport2CoefficientManager &other, double tol=1e-9) const
Compare this Coefficient manager with another for equality.
Key insertCoefficient(Time time, const Coefficient &coefficient)
insert a coefficient at a time and return the key for the 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
void modifyCoefficient(typename TimeToKeyCoefficientMap::iterator it, Time time, const Coefficient &coefficient)
Modify a coefficient by specifying a new time and value.