Curves
A library for curves generation.
SE3CompositionCurve.hpp
Go to the documentation of this file.
1 /*
2  * @file CompositionCurve.hpp
3  * @date Feb 06, 2015
4  * @author Renaud DubĂ©, Abel Gawel, Mike Bosse
5  */
6 
7 #include "curves/SE3Curve.hpp"
8 
9 #pragma once
10 
11 namespace curves {
12 
13 // SE3CompositionCurve is a curve composed of a base and a correction curve.
14 // The corrections can be sampled at a lower frequency than the base curve,
15 // therefore reducing the optimization state space. The corrections are applied
16 // on the left side.
17 
18 template <class C1, class C2>
19 class SE3CompositionCurve : public SE3Curve {
20 
21  private:
24 
25  public:
28 
31 
33  virtual void print(const std::string& str = "") const;
34 
36  void saveCurves(const std::string& filename) const;
37 
39  virtual Time getMinTime() const;
40 
42  virtual Time getMaxTime() const;
43 
45  bool isEmpty() const;
46 
48  int size() const;
49 
51  int baseSize() const;
52 
54  int correctionSize() const;
55 
58  virtual void extend(const std::vector<Time>& times,
59  const std::vector<ValueType>& values,
60  std::vector<Key>* outKeys = NULL);
61 
64  void setMinSamplingPeriod(const Time minSamplingPeriod);
65 
68  void setSamplingRatio(const int ratio);
69 
72  void foldInCorrections();
73 
78  virtual void fitCurve(const std::vector<Time>& times,
79  const std::vector<ValueType>& values,
80  std::vector<Key>* outKeys = NULL);
81 
83  void setCorrectionTimes(const std::vector<Time>& times);
84 
86  virtual ValueType evaluate(Time time) const;
87 
95  virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const;
96 
97  virtual void setTimeRange(Time minTime, Time maxTime);
98 
101 
104 
107 
110 
114  virtual Vector6d evaluateTwistA(Time time);
115 
119  virtual Vector6d evaluateTwistB(Time time);
120 
122  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time);
123 
125  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time);
126 
128  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time);
129 
131  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time);
132 
136  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time);
137 
141  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time);
142 
144  virtual void clear();
145 
148 
150  void setCorrectionCoefficientAtTime(Time time, ValueType value);
151 
153  void transformCurve(const ValueType T);
154 
156  void resetCorrectionCurve(const std::vector<Time>& times);
157 
160  void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values);
161 
163  void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values);
164 
167  void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values);
168 
170  void saveCurveTimesAndValues(const std::string& filename) const;
171 
172  void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const;
173 
174  void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const;
175 
176  void saveCorrectionCurveTimesAndValues(const std::string& filename) const;
177 
178  void getBaseCurveTimes(std::vector<Time>* outTimes) const;
179 
180  void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const;
181 
182  void getCurveTimes(std::vector<Time>* outTimes) const;
183 };
184 
185 } // namespace curves
186 
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
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.
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
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.
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
double Time
Definition: Curve.hpp:13
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
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.
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
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.
Parent::ValueType ValueType
Definition: SE3Curve.hpp:30
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.