Curves
A library for curves generation.
SE3CompositionCurve-inl.hpp
Go to the documentation of this file.
1 /*
2  * @file CompositionCurve-inl.hpp
3  * @date Feb 06, 2015
4  * @author Renaud DubĂ©, Abel Gawel, Mike Bosse
5  */
6 
7 // COMPOSITION_STRATEGY is used to select the strategy for composing the
8 // correction curve with the base curve at the evaluation time t:
9 // (1) corr(t) * base(t) is implemented
10 // (2) corr is evaluated at the coefficient times (t1, t2) where the interpolation
11 // on base is computed resulting in
12 // interpolation(corr(t1) * base(t1), corr(t2) * base(t2), alpha)
13 
14 #define COMPOSITION_STRATEGY 1
15 
17 #include "curves/helpers.hpp"
18 
19 namespace curves{
20 
21 template <class C1, class C2>
23 
24 }
25 
26 template <class C1, class C2>
28 
29 }
30 template <class C1, class C2>
31 void SE3CompositionCurve<C1, C2>::print(const std::string& str) const {
32  std::cout << "=========================================" << std::endl;
33  std::cout << "===== CompositionCurve SE3 CURVE ========" << std::endl;
34  std::cout << str << std::endl;
35  baseCurve_.print("Base curve");
36  correctionCurve_.print("Correction curve");
37 }
38 
39 template <class C1, class C2>
40 void SE3CompositionCurve<C1, C2>::saveCurves(const std::string& filename) const {
41  std::vector<Time> baseCurveTimes;
42  baseCurve_.manager_.getTimes(&baseCurveTimes);
43 
44  Eigen::VectorXd v(7);
45 
46  std::vector<Eigen::VectorXd> baseCurveValues;
47  for (size_t i = 0; i < baseCurveTimes.size(); ++i) {
48  ValueType val = baseCurve_.evaluate(baseCurveTimes[i]);
49  v << val.getPosition().x(), val.getPosition().y(), val.getPosition().z(),
50  val.getRotation().w(), val.getRotation().x(), val.getRotation().y(), val.getRotation().z();
51  baseCurveValues.push_back(v);
52  }
53 
54  std::vector<Time> correctionCurveTimes;
55  correctionCurve_.manager_.getTimes(&correctionCurveTimes);
56  std::vector<Eigen::VectorXd> correctionCurveValues;
57  for (size_t i = 0; i < correctionCurveTimes.size(); ++i) {
58  ValueType val = correctionCurve_.evaluate(correctionCurveTimes[i]);
59  v << val.getPosition().x(), val.getPosition().y(), val.getPosition().z(),
60  val.getRotation().w(), val.getRotation().x(), val.getRotation().y(), val.getRotation().z();
61  correctionCurveValues.push_back(v);
62  }
63 
64  std::vector<Eigen::VectorXd> combinedCurveValues;
65  for (size_t i = 0; i < baseCurveTimes.size(); ++i) {
66  ValueType val = this->evaluate(baseCurveTimes[i]);
67  v << val.getPosition().x(), val.getPosition().y(), val.getPosition().z(),
68  val.getRotation().w(), val.getRotation().x(), val.getRotation().y(), val.getRotation().z();
69  combinedCurveValues.push_back(v);
70  }
71 
72  writeTimeVectorCSV(filename + "_base.csv", baseCurveTimes, baseCurveValues);
73  writeTimeVectorCSV(filename + "_correction.csv", correctionCurveTimes, correctionCurveValues);
74  writeTimeVectorCSV(filename + "_composed.csv", baseCurveTimes, combinedCurveValues);
75 
76 }
77 
78 template <class C1, class C2>
80  return baseCurve_.getMinTime();
81 }
82 
83 template <class C1, class C2>
85  return baseCurve_.getMaxTime();
86 }
87 
88 template <class C1, class C2>
90  return baseCurve_.isEmpty();
91 }
92 
93 template <class C1, class C2>
95 // return baseCurve_.size();
96  return correctionCurve_.size();
97 }
98 
99 template <class C1, class C2>
101  return baseCurve_.size();
102 }
103 
104 template <class C1, class C2>
106  return correctionCurve_.size();
107 }
108 
109 template <class C1, class C2>
111  baseCurve_.setMinSamplingPeriod(0);
112  correctionCurve_.setMinSamplingPeriod(minSamplingPeriod);
113 }
114 
115 template <class C1, class C2>
117  baseCurve_.setSamplingRatio(1);
118  correctionCurve_.setSamplingRatio(ratio);
119 }
120 
121 template <class C1, class C2>
122 void SE3CompositionCurve<C1, C2>::extend(const std::vector<Time>& times,
123  const std::vector<typename SE3CompositionCurve<C1, C2>::ValueType>& values,
124  std::vector<Key>* outKeys) {
125  //todo: Treat case when times.size() != 1
126  CHECK_EQ(times.size(), 1) << "Extend was called with more than one time.";
127  CHECK_EQ(values.size(), 1) << "Extend was called with more than one value.";
128 
129  // Find the new limit times of the curve
130  Time newMaxTime, newMinTime;
131 
132  if (baseCurve_.isEmpty()) {
133  newMaxTime = 0;
134  newMinTime = 0;
135  } else {
136  newMaxTime = baseCurve_.getMaxTime();
137  newMinTime = baseCurve_.getMinTime();
138  }
139 
140  for (std::vector<Time>::const_iterator it = times.begin(); it != times.end(); ++it) {
141  if (newMaxTime < *it) {
142  newMaxTime = *it;
143  }
144  if (newMinTime > *it) {
145  newMinTime = *it;
146  }
147  }
148 
149  // Extend the correction curves to these times
150  std::vector<Time> correctionTimes;
151  std::vector<ValueType> correctionValues;
152 
153  if (correctionCurve_.isEmpty()) {
154  correctionTimes.push_back(newMinTime);
155  correctionValues.push_back(ValueType(ValueType::Position(0,0,0), ValueType::Rotation(1,0,0,0)));
156  correctionCurve_.extend(correctionTimes, correctionValues);
157  }
158 
159  if (correctionCurve_.getMaxTime() < newMaxTime) {
160  correctionTimes.push_back(newMaxTime);
161  correctionValues.push_back(correctionCurve_.evaluate(correctionCurve_.getMaxTime()));
162  correctionCurve_.extend(correctionTimes, correctionValues);
163  }
164 
165  if (correctionCurve_.getMinTime() > newMinTime) {
166  correctionTimes.push_back(newMinTime);
167  correctionValues.push_back(correctionCurve_.evaluate(correctionCurve_.getMinTime()));
168  correctionCurve_.extend(correctionTimes, correctionValues);
169  }
170 
171  //Compute the base curve updates accounting for the corrections
172  std::vector<ValueType> newValues;
173  std::vector<ValueType>::const_iterator itValues = values.begin();
174  for (std::vector<Time>::const_iterator it = times.begin(); it != times.end(); ++it) {
175  newValues.push_back(correctionCurve_.evaluate(*it).inverted() * (*itValues));
176  ++itValues;
177  }
178  baseCurve_.extend(times, newValues, outKeys);
179 }
180 
181 template <class C1, class C2>
183  std::cout << "foldincorrections" << std::endl;
184  std::vector<Time> times;
185  std::vector<ValueType> newValues;
186  baseCurve_.manager_.getTimes(&times);
187  for (std::vector<Time>::const_iterator it = times.begin(); it != times.end(); ++it) {
188  newValues.push_back(this->evaluate(*it));
189  }
190  baseCurve_.clear();
191  baseCurve_.extend(times,newValues);
192  times.clear();
193  newValues.clear();
194  correctionCurve_.manager_.getTimes(&times);
195  for (std::vector<Time>::const_iterator it = times.begin(); it != times.end(); ++it) {
196  newValues.push_back(ValueType(ValueType::Position(0,0,0), ValueType::Rotation(1,0,0,0)));
197  }
198  correctionCurve_.clear();
199  correctionCurve_.extend(times,newValues);
200 }
201 
202 template <class C1, class C2>
203 void SE3CompositionCurve<C1, C2>::fitCurve(const std::vector<Time>& times,
204  const std::vector<typename SE3CompositionCurve<C1, C2>::ValueType>& values,
205  std::vector<Key>* outKeys){
206  extend(times, values, outKeys);
207 }
208 
209 template <class C1, class C2>
210 void SE3CompositionCurve<C1, C2>::setCorrectionTimes(const std::vector<Time>& times) {
211  // Evaluate the correction curve at these times
212  std::vector<ValueType> values;
213  for (size_t i = 0; i < times.size(); ++i) {
214  values.push_back(correctionCurve_.evaluate(times[i]));
215  }
216 
217  // Redefine the correction curve
218  correctionCurve_.clear();
219  correctionCurve_.extend(times, values);
220 
221  CHECK_EQ(correctionCurve_.getMinTime(), baseCurve_.getMinTime()) << "Min time of correction curve and base curve are different";
222  CHECK_EQ(correctionCurve_.getMaxTime(), baseCurve_.getMaxTime()) << "Min time of correction curve and base curve are different";
223 }
224 
225 template <class C1, class C2>
227 
228 #if COMPOSITION_STRATEGY == 1
229  // (1) corr(t) * base(t) is implemented
230  return correctionCurve_.evaluate(time) * baseCurve_.evaluate(time);
231 #elif COMPOSITION_STRATEGY == 2
232  // (2) corr is evaluated at the coefficient times (t1, t2) where the interpolation
233  // on base is computed resulting in
234  // interpolation(corr(t1) * base(t1), corr(t2) * base(t2), alpha)
235  if (time == 0) {
236  return correctionCurve_.evaluate(time) * baseCurve_.evaluate(time);
237  }
238 
239  typename C2::CoefficientIter it1, it2;
240  baseCurve_.manager_.getCoefficientsAt(time, &it1, &it2);
241  //todo Is it true that the value of a curve at a time where a coefficient
242  // is defined is always only dependent on this coefficient?
243  if(it1->first == time || it2->first == time) {
244  return correctionCurve_.evaluate(time) * baseCurve_.evaluate(time);
245  } else {
246  Time tA, tB;
247  ValueType A, B, dA, dB, T_W_A, T_W_B;
248  //if the iterators returned are in order ( last element bug)
249  if (it1->first < it2->first) {
250  tA = it1->first;
251  tB = it2->first;
252  } else {
253  tA = it2->first;
254  tB = it1->first;
255  }
256 
257  double alpha = double(time - tA)/double(tB - tA);
258 
259  A = baseCurve_.evaluate(tA);
260  B = baseCurve_.evaluate(tB);
261  dA = correctionCurve_.evaluate(tA);
262  dB = correctionCurve_.evaluate(tB);
263 
264  T_W_A = dA * A;
265  T_W_B = dB * B;
266 
267  //Implementation of T_W_I = T_W_A*exp(alpha*log(inv(T_W_A)*T_W_B))
268  using namespace kindr::minimal;
269  ValueType T_A_B = invertAndComposeImplementation(T_W_A, T_W_B, boost::none, boost::none);
270  gtsam::Vector6 log_T_A_B = transformationLogImplementation(T_A_B, boost::none);
271  gtsam::Vector6 log_T_A_I = vectorScalingImplementation<int(6)>(log_T_A_B, alpha, boost::none, boost::none);
272  ValueType T_A_I = transformationExpImplementation(log_T_A_I, boost::none);
273  return composeImplementation(T_W_A, T_A_I, boost::none, boost::none);
274  }
275 #endif
276 }
277 
278 template <class C1, class C2>
280  unsigned derivativeOrder) const{
281  //todo
282 }
283 
284 template <class C1, class C2>
286  //todo
287 }
288 
289 template <class C1, class C2>
291  //todo
292 }
293 
294 template <class C1, class C2>
296  //todo
297 }
298 
299 template <class C1, class C2>
301  //todo
302 }
303 
304 template <class C1, class C2>
306  //todo
307 }
308 
309 template <class C1, class C2>
311  //todo
312 }
313 
314 template <class C1, class C2>
316  //todo
317 }
318 
319 template <class C1, class C2>
321  //todo
322 }
323 
324 template <class C1, class C2>
326  //todo
327 }
328 
329 template <class C1, class C2>
331  //todo
332 }
333 
334 template <class C1, class C2>
336  //todo
337 }
338 
339 template <class C1, class C2>
341  //todo
342 }
343 
344 template <class C1, class C2>
346  //todo
347 }
348 
349 template <class C1, class C2>
351  baseCurve_.clear();
352  correctionCurve_.clear();
353 }
354 
355 template <class C1, class C2>
357  CHECK(correctionCurve_.manager_.hasCoefficientAtTime(time));
358  correctionCurve_.manager_.removeCoefficientAtTime(time);
359 }
360 template <class C1, class C2>
362  CHECK(correctionCurve_.manager_.hasCoefficientAtTime(time));
363  correctionCurve_.manager_.insertCoefficient(time, value);
364 }
365 
366 template <class C1, class C2>
368  // Apply the transformation on the left side
369  // todo here we assume that the correctionCurve is identity.
370  baseCurve_.transformCurve(T);
371 }
372 
373 template <class C1, class C2>
374 void SE3CompositionCurve<C1, C2>::resetCorrectionCurve(const std::vector<Time>& times) {
375  std::vector<ValueType> values;
376  for (size_t i = 0; i < times.size(); ++i) {
377  values.push_back(ValueType(ValueType::Position(0,0,0), ValueType::Rotation(1,0,0,0)));
378  }
379 
380  // Redefine the correction curve
381  correctionCurve_.fitCurve(times, values);
382 
383  CHECK_EQ(correctionCurve_.getMinTime(), baseCurve_.getMinTime()) << "Min time of correction curve and base curve are different";
384  CHECK_EQ(correctionCurve_.getMaxTime(), baseCurve_.getMaxTime()) << "Min time of correction curve and base curve are different";
385 }
386 
387 template <class C1, class C2>
388 void SE3CompositionCurve<C1, C2>::setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) {
389  baseCurve_.fitCurve(times, values);
390  CHECK_EQ(correctionCurve_.getMinTime(), baseCurve_.getMinTime()) << "Min time of correction curve and base curve are different";
391  CHECK_EQ(correctionCurve_.getMaxTime(), baseCurve_.getMaxTime()) << "Min time of correction curve and base curve are different";
392 }
393 
394 template <class C1, class C2>
395 void SE3CompositionCurve<C1, C2>::setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) {
396  baseCurve_.setCurve(times, values);
397  CHECK_EQ(correctionCurve_.getMinTime(), baseCurve_.getMinTime()) << "Min time of correction curve and base curve are different";
398  CHECK_EQ(correctionCurve_.getMaxTime(), baseCurve_.getMaxTime()) << "Min time of correction curve and base curve are different";
399 }
400 
401 template <class C1, class C2>
402 void SE3CompositionCurve<C1, C2>::modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) {
403  baseCurve_.manager_.modifyCoefficientsValuesInBatch(times, values);
404  CHECK_EQ(correctionCurve_.getMinTime(), baseCurve_.getMinTime()) << "Min time of correction curve and base curve are different";
405  CHECK_EQ(correctionCurve_.getMaxTime(), baseCurve_.getMaxTime()) << "Min time of correction curve and base curve are different";
406 }
407 
408 template <class C1, class C2>
409 void SE3CompositionCurve<C1, C2>::saveCurveTimesAndValues(const std::string& filename) const {
410  std::vector<Time> curveTimes;
411  baseCurve_.manager_.getTimes(&curveTimes);
412 
413  saveCurveAtTimes(filename, curveTimes);
414 }
415 
416 template <class C1, class C2>
417 void SE3CompositionCurve<C1, C2>::saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const {
418  Eigen::VectorXd v(7);
419 
420  std::vector<Eigen::VectorXd> curveValues;
421  ValueType val;
422  for (size_t i = 0; i < times.size(); ++i) {
423  val = evaluate(times[i]);
424  v << val.getPosition().x(), val.getPosition().y(), val.getPosition().z(),
425  val.getRotation().w(), val.getRotation().x(), val.getRotation().y(), val.getRotation().z();
426  curveValues.push_back(v);
427  }
428 
429  writeTimeVectorCSV(filename, times, curveValues);
430 }
431 
432 template <class C1, class C2>
433 void SE3CompositionCurve<C1, C2>::saveCorrectionCurveTimesAndValues(const std::string& filename) const {
434  std::vector<Time> curveTimes;
435  correctionCurve_.manager_.getTimes(&curveTimes);
436 
437  saveCorrectionCurveAtTimes(filename, curveTimes);
438 }
439 
440 template <class C1, class C2>
441 void SE3CompositionCurve<C1, C2>::saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const {
442  Eigen::VectorXd v(7);
443 
444  std::vector<Eigen::VectorXd> curveValues;
445  ValueType val;
446  for (size_t i = 0; i < times.size(); ++i) {
447  val = correctionCurve_.evaluate(times[i]);
448  v << val.getPosition().x(), val.getPosition().y(), val.getPosition().z(),
449  val.getRotation().w(), val.getRotation().x(), val.getRotation().y(), val.getRotation().z();
450  curveValues.push_back(v);
451  }
452 
453  writeTimeVectorCSV(filename, times, curveValues);
454 }
455 
456 template <class C1, class C2>
457 void SE3CompositionCurve<C1, C2>::getBaseCurveTimes(std::vector<Time>* outTimes) const {
458  baseCurve_.manager_.getTimes(outTimes);
459 }
460 
461 template <class C1, class C2>
463  Time begTime, Time endTime) const {
464  baseCurve_.manager_.getTimesInWindow(outTimes, begTime, endTime);
465 }
466 
467 template <class C1, class C2>
468 void SE3CompositionCurve<C1, C2>::getCurveTimes(std::vector<Time>* outTimes) const {
469  correctionCurve_.manager_.getTimes(outTimes);
470 }
471 
472 } // namespace curves
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
SE2 composeImplementation(const SE2 &T1, const SE2 &T2, ChartJacobian HT1, ChartJacobian HT2)
int correctionSize() const
Returns the number of coefficients in the correction curve.
virtual Time getMaxTime() const
Returns the last valid time for the curve.
int baseSize() const
Returns the number of coefficients in the base curve.
void setBaseCurve(const std::vector< Time > &times, const std::vector< ValueType > &values)
Set the base curve to given values with knots at desired times Resets the curve beforehand.
static void writeTimeVectorCSV(std::string fileName, const std::vector< curves::Time > &times, const std::vector< Eigen::VectorXd > &values)
Helper function to write CSV file formatted in: time, vectorEntry0, vectorEntry1, ...
Definition: helpers.hpp:86
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b...
void saveCurveTimesAndValues(const std::string &filename) const
Save the base curve times and composed curve values.
virtual Eigen::Vector3d evaluateLinearVelocityB(Time time)
Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
virtual void setTimeRange(Time minTime, Time maxTime)
void saveCorrectionCurveTimesAndValues(const std::string &filename) const
virtual Vector6d evaluateTwistA(Time time)
evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a...
void getCurveTimes(std::vector< Time > *outTimes) const
void getBaseCurveTimes(std::vector< Time > *outTimes) const
SE3Curve::ValueType ValueType
virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time)
evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a...
virtual Eigen::Vector3d evaluateLinearVelocityA(Time time)
Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.
void modifyBaseCoefficientsValuesInBatch(const std::vector< Time > &times, const std::vector< ValueType > &values)
Modifies values of the base coefficient in batch, starting at times[0] and assuming that a coefficien...
void resetCorrectionCurve(const std::vector< Time > &times)
Reset the correction curve to identity values with knots at desired times.
virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const
void saveCurves(const std::string &filename) const
Save curves data as .csv file.
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
SE2 transformationExpImplementation(const Vector3 &params, ChartJacobian Hp)
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
Vector3 transformationLogImplementation(const SE2 &T, ChartJacobian HT)
SE3 invertAndComposeImplementation(SE3 A, SE3 B)
double Time
Definition: Curve.hpp:13
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
Eigen::Matrix< double, 6, 1 > Vector6
void setMinSamplingPeriod(const Time minSamplingPeriod)
Set the minimum sampling period for the correction curve. Overloads the function defined in SE3Curve ...
int size() const
Returns the number of coefficients in the correction curve.
void setSamplingRatio(const int ratio)
Set the sampling ratio for the correction curve. eg. 4 will add a coefficient every 4 extend...
void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
virtual Eigen::Vector3d evaluateAngularVelocityB(Time time)
Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b...
bool isEmpty() const
Checks if the curve is empty.
virtual Eigen::Vector3d evaluateAngularVelocityA(Time time)
Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a...
virtual void clear()
Clear the base and correction curves.
void setCorrectionCoefficientAtTime(Time time, ValueType value)
Set the correction coefficient value at the specified time.
virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a...
virtual Time getMinTime() const
Returns the first valid time for the curve.
void saveCurveAtTimes(const std::string &filename, std::vector< Time > times) const
virtual void fitCurve(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
Fit a new curve to these data points.
virtual Vector6d evaluateTwistB(Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
SE3Curve::DerivativeType DerivativeType
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)
Add / replace the given coefficients without resetting the curve.
void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
virtual void print(const std::string &str="") const
Print the value of the base and corrections curves coefficients.
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
Extend the curve so that it can be evaluated at these times by using a default correction sampling po...
void removeCorrectionCoefficientAtTime(Time time)
Remove a correction coefficient at the specified time.