Curves
A library for curves generation.
SemiDiscreteSE3Curve.hpp
Go to the documentation of this file.
1 /*
2  * @file SemiDiscreteSE3Curve.hpp
3  * @date Feb 3, 2016
4  * @author Renaud DubĂ©, Abel Gawel
5  */
6 
7 #ifndef CURVES_SEMI_DISCRETE_SE3_CURVE_HPP
8 #define CURVES_SEMI_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 
22  friend class SamplingPolicy;
23  public:
27 
29  virtual ~SemiDiscreteSE3Curve();
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 
214 // extend policy for slerp curves
215 template<>
216 inline void SamplingPolicy::extend<SemiDiscreteSE3Curve, SE3>(const std::vector<Time>& times,
217  const std::vector<SE3>& values,
218  SemiDiscreteSE3Curve* curve,
219  std::vector<Key>* outKeys) {
220  //todo: deal with minSamplingPeriod_ when extending with multiple times
221  if (times.size() != 1) {
222  curve->manager_.insertCoefficients(times, values, outKeys);
223  } else {
224  //If the curve is empty or of size 1, simply add the new coefficient
225  if (curve->isEmpty() || curve->size() == 1) {
226  curve->manager_.insertCoefficients(times, values, outKeys);
227  } else {
228  if (minimumMeasurements_ == 1) {
229  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
230  } else {
231  ++measurementsSinceLastExtend_;
232 
233  if (measurementsSinceLastExtend_ == 1) {
234  curve->manager_.addCoefficientAtEnd(times[0], values[0], outKeys);
235  } else {
236  SemiDiscreteSE3Curve::TimeToKeyCoefficientMap::iterator itPrev = (--curve->manager_.coefficientEnd());
237  curve->manager_.modifyCoefficient(itPrev, times[0], values[0]);
238  }
239  if (measurementsSinceLastExtend_ == minimumMeasurements_) {
240  measurementsSinceLastExtend_ = 0;
241  }
242  }
243  }
244  }
245 }
246 
247 } // namespace curves
248 
249 #endif /* CURVES_DISCRETE_SE3_CURVE_HPP */
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 setTimeRange(Time minTime, Time maxTime)
virtual Time getMinTime() const
The first valid time for the curve.
virtual void updateFromGTSAMValues(const gtsam::Values &values)
void saveCorrectionCurveAtTimes(const std::string &filename, std::vector< Time > times) const
void getCurveTimes(std::vector< Time > *outTimes) const
virtual void extend(const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
virtual Eigen::Vector3d evaluateLinearVelocityA(Time time)
Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.
void transformCurve(const ValueType T)
Perform a rigid transformation on the left side of the curve.
virtual void clear()
Clear all the curve coefficients.
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.
void foldInCorrections()
Fold in the correction curve into the base curve and reinitialize correction curve coefficients to id...
virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a...
void setBaseCurvePart(const std::vector< Time > &times, const std::vector< ValueType > &values)
Add / replace the given coefficients without resetting the curve.
virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: SE3Config.hpp:16
virtual gtsam::Expression< DerivativeType > getDerivativeExpression(const Time &time, unsigned derivativeOrder) const
void setCorrectionTimes(const std::vector< Time > &times)
Add coefficients to the correction curve at given times.
void saveCurveAtTimes(const std::string &filename, std::vector< Time > times) const
std::map< Time, KeyCoefficient > TimeToKeyCoefficientMap
virtual gtsam::Expression< ValueType > getValueExpression(const Time &time) const
Get an evaluator at this time.
void setSamplingRatio(const int ratio)
Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.
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...
int correctionSize() const
Returns the number of coefficients in the correction curve.
virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
virtual Time getMaxTime() const
The one past the last valid time for the curve.
virtual void initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values *values) const
Initialize a GTSAM values structure with the desired keys.
virtual Eigen::Vector3d evaluateAngularVelocityA(Time time)
Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a...
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 print(const std::string &str="") const
Print the value of the coefficient, for debugging and unit tests.
void setMinSamplingPeriod(Time time)
set the minimum sampling period
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...
kindr::AngleAxisPD AngleAxis
virtual Vector6d evaluateTwistB(Time time)
evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b...
double Time
Definition: Curve.hpp:13
void resetCorrectionCurve(const std::vector< Time > &times)
Reset the correction curve to identity values with knots at desired times.
virtual DerivativeType evaluateDerivative(Time time, unsigned derivativeOrder) const
virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time)
Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b...
SE3::Rotation SO3
Implements a discrete SE3 curve class.
LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
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 Eigen::Vector3d evaluateAngularVelocityB(Time time)
Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b...
void getBaseCurveTimes(std::vector< Time > *outTimes) const
virtual ValueType evaluate(Time time) const
Evaluate the ambient space of the curve.
void removeCorrectionCoefficientAtTime(Time time)
Remove a correction coefficient at the specified time.
Parent::DerivativeType DerivativeType
Definition: SE3Curve.hpp:31
TimeToKeyCoefficientMap::const_iterator CoefficientIter
virtual Time getTimeAtKey(gtsam::Key key) const
void saveCurveTimesAndValues(const std::string &filename) const
virtual Eigen::Vector3d evaluateLinearVelocityB(Time time)
Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
void getBaseCurveTimesInWindow(std::vector< Time > *outTimes, Time begTime, Time endTime) const
size_t Key
Definition: Curve.hpp:14
void addPriorFactors(gtsam::NonlinearFactorGraph *graph, Time priorTime) const
Add factors to constrain the variables active at this time.
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...
LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: SE2Config.hpp:15
void setCorrectionCoefficientAtTime(Time time, ValueType value)
Set the correction coefficient value at the specified time.
kindr::HomogeneousTransformationPosition3RotationQuaternionD SE3
LocalSupport2CoefficientManager< Coefficient > manager_