Curves
A library for curves generation.
DiscreteSE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * @file DiscreteSE3Curve.hpp
3  * @date Feb 3, 2016
4  * @author Renaud DubĂ©, Abel Gawel
5  */
6 
7 #ifndef CURVES_DISCRETE_SE3_CURVE_HPP
8 #define CURVES_DISCRETE_SE3_CURVE_HPP
9 
10 #include "SE3Curve.hpp"
12 #include "SE3CompositionCurve.hpp"
13 #include "gtsam/nonlinear/NonlinearFactorGraph.h"
14 #include "SamplingPolicy.hpp"
15 #include "CubicHermiteSE3Curve.hpp"
16 
17 namespace curves {
18 
20 class DiscreteSE3Curve : public SE3Curve {
22  friend class SamplingPolicy;
23  public:
27 
29  virtual ~DiscreteSE3Curve();
30 
32  virtual void print(const std::string& str = "") const;
33 
35  virtual Time getMinTime() const;
36 
38  virtual Time getMaxTime() const;
39 
40  bool isEmpty() const;
41 
42  // return number of coefficients curve is composed of
43  int size() const;
44 
48  virtual void extend(const std::vector<Time>& times,
49  const std::vector<ValueType>& values,
50  std::vector<Key>* outKeys = NULL);
51 
56  virtual void fitCurve(const std::vector<Time>& times,
57  const std::vector<ValueType>& values,
58  std::vector<Key>* outKeys = NULL);
59 
62  void setCurve(const std::vector<Time>& times,
63  const std::vector<ValueType>& values);
64 
66  virtual ValueType evaluate(Time time) const;
67 
75  virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const;
76 
78  virtual gtsam::Expression<ValueType> getValueExpression(const Time& time) const;
79 
80  virtual gtsam::Expression<DerivativeType> getDerivativeExpression(const Time& time, unsigned derivativeOrder) const;
81 
82  virtual void setTimeRange(Time minTime, Time maxTime);
83 
86 
89 
92 
95 
99  virtual Vector6d evaluateTwistA(Time time);
100 
104  virtual Vector6d evaluateTwistB(Time time);
105 
107  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time);
108 
110  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time);
111 
113  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time);
114 
116  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time);
117 
121  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time);
122 
126  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time);
127 
129  virtual void initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values* values) const;
130 
132  virtual void initializeGTSAMValues(gtsam::Values* values) const;
133 
134  // updates the relevant curve coefficients from the GTSAM values structure
135  virtual void updateFromGTSAMValues(const gtsam::Values& values);
136 
137  // set minimum sampling period
138  void setMinSamplingPeriod(Time time);
139 
142  void setSamplingRatio(const int ratio);
143 
144  virtual void clear();
145 
147  void addPriorFactors(gtsam::NonlinearFactorGraph* graph, Time priorTime) const;
148 
150  void transformCurve(const ValueType T);
151 
152  virtual Time getTimeAtKey(gtsam::Key key) const;
153 
154  void saveCurveTimesAndValues(const std::string& filename) const;
155 
156  void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const;
157 
158  void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const {};
159 
160  void getCurveTimes(std::vector<Time>* outTimes) const;
161 
162  // Fake functions to comply with the current interfaces of trajectories_optimizer
163  // todo : tidy up
164 
166  int correctionSize() const {return 0;};
167 
170  void foldInCorrections() {};
171 
173  void setCorrectionTimes(const std::vector<Time>& times) {};
174 
177 
180 
182  void resetCorrectionCurve(const std::vector<Time>& times) {};
183 
186  void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
187 
189  void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
190 
193  void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) {};
194 
195  void getBaseCurveTimes(std::vector<Time>* outTimes) const {};
196 
197  void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const {};
198 
199  // return number of coefficients curve is composed of
200  int baseSize() const {return size();};
201 
202  void saveCorrectionCurveTimesAndValues(const std::string& filename) const {};
203 
204  private:
207 };
208 
209 typedef kindr::minimal::QuatTransformationTemplate<double> SE3;
210 typedef SE3::Rotation SO3;
211 typedef kindr::minimal::AngleAxisTemplate<double> AngleAxis;
212 
213 // extend policy for slerp curves
214 template<>
215 inline void SamplingPolicy::extend<DiscreteSE3Curve, SE3>(const std::vector<Time>& times,
216  const std::vector<SE3>& values,
217  DiscreteSE3Curve* curve,
218  std::vector<Key>* outKeys) {
219  //todo: deal with minSamplingPeriod_ when extending with multiple times
220  if (times.size() != 1) {
221  curve->manager_.insertCoefficients(times, values, outKeys);
222  } else {
223  //If the curve is empty or of size 1, simply add the new coefficient
224  if (curve->isEmpty() || curve->size() == 1) {
225  curve->manager_.insertCoefficients(times, values, outKeys);
226  } else {
227  if (minimumMeasurements_ == 1) {
228  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
229  } else {
230  ++measurementsSinceLastExtend_;
231 
232  if (measurementsSinceLastExtend_ == 1) {
233  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
234  } else {
235  DiscreteSE3Curve::TimeToKeyCoefficientMap::iterator itPrev = (--curve->manager_.coefficientEnd());
236  curve->manager_.modifyCoefficient(itPrev, times[0], values[0]);
237  }
238  if (measurementsSinceLastExtend_ == minimumMeasurements_) {
239  measurementsSinceLastExtend_ = 0;
240  }
241  }
242  }
243  }
244 }
245 
246 } // namespace curves
247 
248 #endif /* CURVES_DISCRETE_SE3_CURVE_HPP */
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 void initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values *values) const
Initialize a GTSAM values structure with the desired keys.
Implements a discrete SE3 curve class.
void getCurveTimes(std::vector< Time > *outTimes) const
void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
virtual gtsam::Expression< DerivativeType > getDerivativeExpression(const Time &time, unsigned derivativeOrder) const
void addPriorFactors(gtsam::NonlinearFactorGraph *graph, Time priorTime) const
Add factors to constrain the variables active at this time.
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.
virtual void clear()
Clear all the curve coefficients.
void saveCurveTimesAndValues(const std::string &filename) const
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
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 void updateFromGTSAMValues(const gtsam::Values &values)
virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
virtual Eigen::Vector3d evaluateLinearVelocityB(Time time)
Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
virtual Vector6d evaluateTwistA(Time time)
evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a...
virtual void print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
SE3Curve::ValueType ValueType
void setCorrectionCoefficientAtTime(Time time, ValueType value)
Set the correction coefficient value at the specified time.
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b...
LocalSupport2CoefficientManager< Coefficient > manager_
int correctionSize() const
Returns the number of coefficients in the correction curve.
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
void setSamplingRatio(const int ratio)
Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.
virtual Time getMinTime() const
The first valid time for the curve.
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 saveCorrectionCurveTimesAndValues(const std::string &filename) const
kindr::AngleAxisPD AngleAxis
virtual Time getMaxTime() const
The one past the last valid time for the curve.
double Time
Definition: Curve.hpp:13
void removeCorrectionCoefficientAtTime(Time time)
Remove a correction coefficient at the specified time.
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
void getBaseCurveTimes(std::vector< Time > *outTimes) const
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)
Add / replace the given coefficients without resetting the curve.
SE3::Rotation SO3
virtual gtsam::Expression< ValueType > getValueExpression(const Time &time) const
Get an evaluator at this time.
void setMinSamplingPeriod(Time time)
set the minimum sampling period
virtual Vector6d evaluateTwistB(Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
virtual Eigen::Vector3d evaluateAngularVelocityB(Time time)
Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b...
virtual void setTimeRange(Time minTime, Time maxTime)
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 DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const
virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a...
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
TimeToKeyCoefficientMap::const_iterator CoefficientIter
void resetCorrectionCurve(const std::vector< Time > &times)
Reset the correction curve to identity values with knots at desired times.
LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
size_t Key
Definition: Curve.hpp:14
void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
virtual Eigen::Vector3d evaluateAngularVelocityA(Time time)
Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a...
virtual Time getTimeAtKey(gtsam::Key key) const
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time)
evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a...
kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3
void saveCurveAtTimes(const std::string &filename, std::vector< Time > times) const
virtual Eigen::Vector3d evaluateLinearVelocityA(Time time)
Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.