Curves
A library for curves generation.
SlerpSE2Curve.hpp
Go to the documentation of this file.
1 /*
2  * @file SlerpSE2Curve.hpp
3  * @date Nov 24, 2015
4  * @author Renaud DubĂ©, Abel Gawel
5  */
6 
7 #ifndef CURVES_SLERP_SE2_CURVE_HPP
8 #define CURVES_SLERP_SE2_CURVE_HPP
9 
10 #include "SE2Curve.hpp"
12 //#include "SE2CompositionCurve.hpp"
13 #include "gtsam/nonlinear/NonlinearFactorGraph.h"
14 #include "SamplingPolicy.hpp"
15 
16 namespace curves {
17 
21 class SlerpSE2Curve : public SE2Curve {
22  //friend class SE2CompositionCurve<SlerpSE2Curve, SlerpSE2Curve>;
23  //friend class SE2CompositionCurve<SlerpSE2Curve, CubicHermiteSE2Curve>;
24  friend class SamplingPolicy;
25  public:
28  typedef ValueType Coefficient;
31 
32  SlerpSE2Curve();
33  virtual ~SlerpSE2Curve();
34 
36  virtual void print(const std::string& str = "") const;
37 
39  virtual Time getMinTime() const;
40 
42  virtual Time getMaxTime() const;
43 
44  bool isEmpty() const;
45 
46  // return number of coefficients curve is composed of
47  int size() const;
48 
52  virtual void extend(const std::vector<Time>& times,
53  const std::vector<ValueType>& values,
54  std::vector<Key>* outKeys = NULL);
55 
60  virtual void fitCurve(const std::vector<Time>& times,
61  const std::vector<ValueType>& values,
62  std::vector<Key>* outKeys = NULL);
63 
66  void setCurve(const std::vector<Time>& times,
67  const std::vector<ValueType>& values);
68 
70  virtual ValueType evaluate(Time time) const;
71 
79  virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const;
80 
82  virtual gtsam::Expression<ValueType> getValueExpression(const Time& time) const;
83 
84  virtual gtsam::Expression<DerivativeType> getDerivativeExpression(const Time& time, unsigned derivativeOrder) const;
85 
86  virtual void setTimeRange(Time minTime, Time maxTime);
87 
89  virtual void initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values* values) const;
90 
92  virtual void initializeGTSAMValues(gtsam::Values* values) const;
93 
94  // updates the relevant curve coefficients from the GTSAM values structure
95  virtual void updateFromGTSAMValues(const gtsam::Values& values);
96 
97  // set minimum sampling period
98  void setMinSamplingPeriod(Time time);
99 
102  void setSamplingRatio(const int ratio);
103 
104  virtual void clear();
105 
107  void addPriorFactors(gtsam::NonlinearFactorGraph* graph, Time priorTime) const;
108 
110  void transformCurve(const ValueType T);
111 
112  virtual Time getTimeAtKey(gtsam::Key key) const;
113 
114  void saveCurveTimesAndValues(const std::string& filename) const;
115 
116  private:
119 };
120 
121 typedef gtsam::Pose2 SE2;
122 typedef gtsam::Rot2 SO2;
123 
124 SE2 transformationPower(SE2 T, double alpha);
125 
126 SE2 composeTransformations(SE2 A, SE2 B);
127 
128 SE2 inverseTransformation(SE2 T);
129 
130 // extend policy for slerp curves
131 template<>
132 inline void SamplingPolicy::extend<SlerpSE2Curve, SE2>(const std::vector<Time>& times,
133  const std::vector<SE2>& values,
134  SlerpSE2Curve* curve,
135  std::vector<Key>* outKeys) {
136  //todo: deal with minSamplingPeriod_ when extending with multiple times
137  if (times.size() != 1) {
138  curve->manager_.insertCoefficients(times, values, outKeys);
139  } else {
140  //If the curve is empty or of size 1, simply add the new coefficient
141  if (curve->isEmpty() || curve->size() == 1) {
142  curve->manager_.insertCoefficients(times, values, outKeys);
143  } else {
144  if (minimumMeasurements_ == 1) {
145  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
146  } else {
147  ++measurementsSinceLastExtend_;
148 
149  if (measurementsSinceLastExtend_ == 1) {
150  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
151  } else {
152  SlerpSE2Curve::TimeToKeyCoefficientMap::iterator itPrev = (--curve->manager_.coefficientEnd());
153  curve->manager_.modifyCoefficient(itPrev, times[0], values[0]);
154  }
155  if (measurementsSinceLastExtend_ == minimumMeasurements_) {
156  measurementsSinceLastExtend_ = 0;
157  }
158  }
159  }
160  }
161 }
162 
163 } // namespace curves
164 
165 #endif /* CURVES_SLERP_SE2_CURVE_HPP */
virtual Time getTimeAtKey(gtsam::Key key) const
virtual Time getMinTime() const
The first valid time for the curve.
void addPriorFactors(gtsam::NonlinearFactorGraph *graph, Time priorTime) const
Add factors to constrain the variables active at this time.
void setMinSamplingPeriod(Time time)
void setCurve(const std::vector< Time > &times, const std::vector< ValueType > &values)
Set some coefficients of the curve The existing curve will NOT be cleared.
virtual void setTimeRange(Time minTime, Time maxTime)
LocalSupport2CoefficientManager< Coefficient > manager_
SE3 inverseTransformation(SE3 T)
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
void setSamplingRatio(const int ratio)
Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.
virtual gtsam::Expression< DerivativeType > getDerivativeExpression(const Time &time, unsigned derivativeOrder) const
virtual void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
virtual void clear()
Clear all the curve coefficients.
virtual Time getMaxTime() const
The one past the last valid time for the curve.
virtual gtsam::Expression< ValueType > getValueExpression(const Time &time) const
Get an evaluator at this time.
void saveCurveTimesAndValues(const std::string &filename) const
gtsam::Pose2 SE2
SE3 transformationPower(SE3 T, double alpha)
double Time
Definition: Curve.hpp:13
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.
SamplingPolicy slerpPolicy_
Parent::ValueType ValueType
Definition: SE2Curve.hpp:28
virtual void updateFromGTSAMValues(const gtsam::Values &values)
bool isEmpty() const
SE2Curve::DerivativeType DerivativeType
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
SE3 composeTransformations(SE3 A, SE3 B)
LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
TimeToKeyCoefficientMap::const_iterator CoefficientIter
Parent::DerivativeType DerivativeType
Definition: SE2Curve.hpp:29
gtsam::Rot2 SO2
void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
virtual void initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values *values) const
Initialize a GTSAM values structure with the desired keys.
size_t Key
Definition: Curve.hpp:14
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
SE2Curve::ValueType ValueType
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const