Curves
A library for curves generation.
curves::SemiDiscreteSE3Curve Class Reference

Implements a discrete SE3 curve class. More...

#include <SemiDiscreteSE3Curve.hpp>

Inheritance diagram for curves::SemiDiscreteSE3Curve:
Inheritance graph

Public Types

typedef ValueType Coefficient
 
typedef LocalSupport2CoefficientManager< Coefficient >::TimeToKeyCoefficientMap TimeToKeyCoefficientMap
 
typedef LocalSupport2CoefficientManager< Coefficient >::CoefficientIter CoefficientIter
 
- Public Types inherited from curves::SE3Curve
typedef Curve< SE3ConfigParent
 
typedef Parent::ValueType ValueType
 
typedef Parent::DerivativeType DerivativeType
 
- Public Types inherited from curves::Curve< SE3Config >
typedef SE3Config::ValueType ValueType
 The value type of the curve. More...
 
typedef SE3Config::DerivativeType DerivativeType
 The curve's derivative type. More...
 

Public Member Functions

 SemiDiscreteSE3Curve ()
 
virtual ~SemiDiscreteSE3Curve ()
 
virtual void print (const std::string &str="") const
 Print the value of the coefficient, for debugging and unit tests. More...
 
virtual Time getMinTime () const
 The first valid time for the curve. More...
 
virtual Time getMaxTime () const
 The one past the last valid time for the curve. More...
 
bool isEmpty () const
 
int size () const
 
virtual void extend (const std::vector< Time > &times, const std::vector< ValueType > &values, std::vector< Key > *outKeys=NULL)
 
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. More...
 
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. More...
 
virtual ValueType evaluate (Time time) const
 Evaluate the ambient space of the curve. More...
 
virtual DerivativeType evaluateDerivative (Time time, unsigned derivativeOrder) const
 
virtual gtsam::Expression< ValueTypegetValueExpression (const Time &time) const
 Get an evaluator at this time. More...
 
virtual gtsam::Expression< DerivativeTypegetDerivativeExpression (const Time &time, unsigned derivativeOrder) const
 
virtual void setTimeRange (Time minTime, Time maxTime)
 
virtual Eigen::Vector3d evaluateAngularVelocityA (Time time)
 Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a. More...
 
virtual Eigen::Vector3d evaluateAngularVelocityB (Time time)
 Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b. More...
 
virtual Eigen::Vector3d evaluateLinearVelocityA (Time time)
 Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a. More...
 
virtual Eigen::Vector3d evaluateLinearVelocityB (Time time)
 Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b. More...
 
virtual Vector6d evaluateTwistA (Time time)
 evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5). More...
 
virtual Vector6d evaluateTwistB (Time time)
 evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5). More...
 
virtual Eigen::Vector3d evaluateAngularDerivativeA (unsigned derivativeOrder, Time time)
 Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a. More...
 
virtual Eigen::Vector3d evaluateAngularDerivativeB (unsigned derivativeOrder, Time time)
 Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b. More...
 
virtual Eigen::Vector3d evaluateLinearDerivativeA (unsigned derivativeOrder, Time time)
 Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a. More...
 
virtual Eigen::Vector3d evaluateLinearDerivativeB (unsigned derivativeOrder, Time time)
 Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b. More...
 
virtual Vector6d evaluateDerivativeA (unsigned derivativeOrder, Time time)
 evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5). More...
 
virtual Vector6d evaluateDerivativeB (unsigned derivativeOrder, Time time)
 evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5). More...
 
virtual void initializeGTSAMValues (gtsam::KeySet keys, gtsam::Values *values) const
 Initialize a GTSAM values structure with the desired keys. More...
 
virtual void initializeGTSAMValues (gtsam::Values *values) const
 Initialize a GTSAM values structure for all keys. More...
 
virtual void updateFromGTSAMValues (const gtsam::Values &values)
 
void setMinSamplingPeriod (Time time)
 set the minimum sampling period More...
 
void setSamplingRatio (const int ratio)
 Set the sampling ratio. eg. 4 will add a coefficient every 4 extend. More...
 
virtual void clear ()
 Clear all the curve coefficients. More...
 
void addPriorFactors (gtsam::NonlinearFactorGraph *graph, Time priorTime) const
 Add factors to constrain the variables active at this time. More...
 
void transformCurve (const ValueType T)
 Perform a rigid transformation on the left side of the curve. More...
 
virtual Time getTimeAtKey (gtsam::Key key) const
 
void saveCurveTimesAndValues (const std::string &filename) const
 
void saveCurveAtTimes (const std::string &filename, std::vector< Time > times) const
 
void saveCorrectionCurveAtTimes (const std::string &filename, std::vector< Time > times) const
 
void getCurveTimes (std::vector< Time > *outTimes) const
 
int correctionSize () const
 Returns the number of coefficients in the correction curve. More...
 
void foldInCorrections ()
 Fold in the correction curve into the base curve and reinitialize correction curve coefficients to identity transformations. More...
 
void setCorrectionTimes (const std::vector< Time > &times)
 Add coefficients to the correction curve at given times. More...
 
void removeCorrectionCoefficientAtTime (Time time)
 Remove a correction coefficient at the specified time. More...
 
void setCorrectionCoefficientAtTime (Time time, ValueType value)
 Set the correction coefficient value at the specified time. More...
 
void resetCorrectionCurve (const std::vector< Time > &times)
 Reset the correction curve to identity values with knots at desired times. More...
 
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. More...
 
void setBaseCurvePart (const std::vector< Time > &times, const std::vector< ValueType > &values)
 Add / replace the given coefficients without resetting the curve. More...
 
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 coefficient exists at all the specified times. More...
 
void getBaseCurveTimes (std::vector< Time > *outTimes) const
 
void getBaseCurveTimesInWindow (std::vector< Time > *outTimes, Time begTime, Time endTime) const
 
int baseSize () const
 
void saveCorrectionCurveTimesAndValues (const std::string &filename) const
 
- Public Member Functions inherited from curves::SE3Curve
 SE3Curve ()
 
virtual ~SE3Curve ()
 
virtual void transformCurve (const ValueType T)=0
 Perform a rigid transformation on the left side of the curve. More...
 
virtual void setCorrectionCoefficientAtTime (Time time, ValueType value)=0
 Set the correction coefficient value at the specified time. More...
 
virtual void setBaseCurve (const std::vector< Time > &times, const std::vector< ValueType > &values)=0
 Set the base curve to given values with knots at desired times Resets the curve beforehand. More...
 
virtual void setBaseCurvePart (const std::vector< Time > &times, const std::vector< ValueType > &values)=0
 Add / replace the given coefficients without resetting the curve. More...
 
virtual void modifyBaseCoefficientsValuesInBatch (const std::vector< Time > &times, const std::vector< ValueType > &values)=0
 Modifies values of the base coefficient in batch, starting at times[0] and assuming that a coefficient exists at all the specified times. More...
 
- Public Member Functions inherited from curves::Curve< SE3Config >
 Curve ()
 
virtual ~Curve ()
 
virtual bool evaluate (ValueType &value, Time time) const =0
 Evaluate the ambient space of the curve. More...
 
virtual bool evaluateDerivative (DerivativeType &derivative, Time time, unsigned derivativeOrder) const =0
 

Private Attributes

LocalSupport2CoefficientManager< Coefficientmanager_
 
SamplingPolicy discretePolicy_
 

Friends

class SE3CompositionCurve< SemiDiscreteSE3Curve, SemiDiscreteSE3Curve >
 
class SamplingPolicy
 

Detailed Description

Implements a discrete SE3 curve class.

Definition at line 20 of file SemiDiscreteSE3Curve.hpp.

Member Typedef Documentation

Constructor & Destructor Documentation

curves::SemiDiscreteSE3Curve::SemiDiscreteSE3Curve ( )
virtual curves::SemiDiscreteSE3Curve::~SemiDiscreteSE3Curve ( )
virtual

Member Function Documentation

void curves::SemiDiscreteSE3Curve::addPriorFactors ( gtsam::NonlinearFactorGraph *  graph,
Time  priorTime 
) const

Add factors to constrain the variables active at this time.

int curves::SemiDiscreteSE3Curve::baseSize ( ) const
inlinevirtual

Implements curves::SE3Curve.

Definition at line 200 of file SemiDiscreteSE3Curve.hpp.

virtual void curves::SemiDiscreteSE3Curve::clear ( )
virtual

Clear all the curve coefficients.

Implements curves::SE3Curve.

int curves::SemiDiscreteSE3Curve::correctionSize ( ) const
inlinevirtual

Returns the number of coefficients in the correction curve.

Implements curves::SE3Curve.

Definition at line 166 of file SemiDiscreteSE3Curve.hpp.

virtual ValueType curves::SemiDiscreteSE3Curve::evaluate ( Time  time) const
virtual

Evaluate the ambient space of the curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateAngularDerivativeA ( unsigned  derivativeOrder,
Time  time 
)
virtual

Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateAngularDerivativeB ( unsigned  derivativeOrder,
Time  time 
)
virtual

Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateAngularVelocityA ( Time  time)
virtual

Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateAngularVelocityB ( Time  time)
virtual

Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b.

Implements curves::SE3Curve.

virtual DerivativeType curves::SemiDiscreteSE3Curve::evaluateDerivative ( Time  time,
unsigned  derivativeOrder 
) const
virtual

Evaluate the curve derivatives. linear 1st derivative has following behaviour:

  • time is out of bound –> error
  • time is between 2 coefficients –> take slope between the 2 coefficients
  • time is on coefficient (not last coefficient) –> take slope between coefficient and next coefficients
  • time is on last coefficient –> take slope between last-1 and last coefficient derivatives of order >1 equal 0
virtual Vector6d curves::SemiDiscreteSE3Curve::evaluateDerivativeA ( unsigned  derivativeOrder,
Time  time 
)
virtual

evaluate the velocity/angular derivative of Frame b as seen from Frame a, expressed in Frame a. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5).

Implements curves::SE3Curve.

virtual Vector6d curves::SemiDiscreteSE3Curve::evaluateDerivativeB ( unsigned  derivativeOrder,
Time  time 
)
virtual

evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5).

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateLinearDerivativeA ( unsigned  derivativeOrder,
Time  time 
)
virtual

Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateLinearDerivativeB ( unsigned  derivativeOrder,
Time  time 
)
virtual

Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateLinearVelocityA ( Time  time)
virtual

Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.

Implements curves::SE3Curve.

virtual Eigen::Vector3d curves::SemiDiscreteSE3Curve::evaluateLinearVelocityB ( Time  time)
virtual

Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.

Implements curves::SE3Curve.

virtual Vector6d curves::SemiDiscreteSE3Curve::evaluateTwistA ( Time  time)
virtual

evaluate the velocity/angular velocity of Frame b as seen from Frame a, expressed in Frame a. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5).

Implements curves::SE3Curve.

virtual Vector6d curves::SemiDiscreteSE3Curve::evaluateTwistB ( Time  time)
virtual

evaluate the velocity/angular velocity of Frame a as seen from Frame b, expressed in Frame b. The return value has the linear velocity (0,1,2), and the angular velocity (3,4,5).

Implements curves::SE3Curve.

virtual void curves::SemiDiscreteSE3Curve::extend ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values,
std::vector< Key > *  outKeys = NULL 
)
virtual

Extend the curve so that it can be evaluated at these times. Try to make the curve fit to the values. Underneath the curve should have some default policy for fitting.

Implements curves::Curve< SE3Config >.

virtual void curves::SemiDiscreteSE3Curve::fitCurve ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values,
std::vector< Key > *  outKeys = NULL 
)
virtual

Fit a new curve to these data points.

The existing curve will be cleared. Underneath the curve should have some default policy for fitting.

Implements curves::Curve< SE3Config >.

void curves::SemiDiscreteSE3Curve::foldInCorrections ( )
inlinevirtual

Fold in the correction curve into the base curve and reinitialize correction curve coefficients to identity transformations.

Implements curves::SE3Curve.

Definition at line 170 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::getBaseCurveTimes ( std::vector< Time > *  outTimes) const
inlinevirtual

Implements curves::SE3Curve.

Definition at line 195 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::getBaseCurveTimesInWindow ( std::vector< Time > *  outTimes,
Time  begTime,
Time  endTime 
) const
inlinevirtual

Implements curves::SE3Curve.

Definition at line 197 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::getCurveTimes ( std::vector< Time > *  outTimes) const
virtual

Implements curves::SE3Curve.

virtual gtsam::Expression<DerivativeType> curves::SemiDiscreteSE3Curve::getDerivativeExpression ( const Time time,
unsigned  derivativeOrder 
) const
virtual
virtual Time curves::SemiDiscreteSE3Curve::getMaxTime ( ) const
virtual

The one past the last valid time for the curve.

Implements curves::Curve< SE3Config >.

virtual Time curves::SemiDiscreteSE3Curve::getMinTime ( ) const
virtual

The first valid time for the curve.

Implements curves::Curve< SE3Config >.

virtual Time curves::SemiDiscreteSE3Curve::getTimeAtKey ( gtsam::Key  key) const
virtual
virtual gtsam::Expression<ValueType> curves::SemiDiscreteSE3Curve::getValueExpression ( const Time time) const
virtual

Get an evaluator at this time.

virtual void curves::SemiDiscreteSE3Curve::initializeGTSAMValues ( gtsam::KeySet  keys,
gtsam::Values *  values 
) const
virtual

Initialize a GTSAM values structure with the desired keys.

virtual void curves::SemiDiscreteSE3Curve::initializeGTSAMValues ( gtsam::Values *  values) const
virtual

Initialize a GTSAM values structure for all keys.

bool curves::SemiDiscreteSE3Curve::isEmpty ( ) const
virtual

Implements curves::SE3Curve.

void curves::SemiDiscreteSE3Curve::modifyBaseCoefficientsValuesInBatch ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values 
)
inline

Modifies values of the base coefficient in batch, starting at times[0] and assuming that a coefficient exists at all the specified times.

Definition at line 193 of file SemiDiscreteSE3Curve.hpp.

virtual void curves::SemiDiscreteSE3Curve::print ( const std::string &  str = "") const
virtual

Print the value of the coefficient, for debugging and unit tests.

Implements curves::Curve< SE3Config >.

void curves::SemiDiscreteSE3Curve::removeCorrectionCoefficientAtTime ( Time  time)
inlinevirtual

Remove a correction coefficient at the specified time.

Implements curves::SE3Curve.

Definition at line 176 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::resetCorrectionCurve ( const std::vector< Time > &  times)
inlinevirtual

Reset the correction curve to identity values with knots at desired times.

Implements curves::SE3Curve.

Definition at line 182 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::saveCorrectionCurveAtTimes ( const std::string &  filename,
std::vector< Time times 
) const
inlinevirtual

Implements curves::SE3Curve.

Definition at line 158 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::saveCorrectionCurveTimesAndValues ( const std::string &  filename) const
inlinevirtual

Implements curves::SE3Curve.

Definition at line 202 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::saveCurveAtTimes ( const std::string &  filename,
std::vector< Time times 
) const
virtual

Implements curves::SE3Curve.

void curves::SemiDiscreteSE3Curve::saveCurveTimesAndValues ( const std::string &  filename) const
virtual

Implements curves::SE3Curve.

void curves::SemiDiscreteSE3Curve::setBaseCurve ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values 
)
inline

Set the base curve to given values with knots at desired times Resets the curve beforehand.

Definition at line 186 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::setBaseCurvePart ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values 
)
inline

Add / replace the given coefficients without resetting the curve.

Definition at line 189 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::setCorrectionCoefficientAtTime ( Time  time,
ValueType  value 
)
inline

Set the correction coefficient value at the specified time.

Definition at line 179 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::setCorrectionTimes ( const std::vector< Time > &  times)
inlinevirtual

Add coefficients to the correction curve at given times.

Implements curves::SE3Curve.

Definition at line 173 of file SemiDiscreteSE3Curve.hpp.

void curves::SemiDiscreteSE3Curve::setCurve ( const std::vector< Time > &  times,
const std::vector< ValueType > &  values 
)

Set some coefficients of the curve The existing curve will NOT be cleared.

void curves::SemiDiscreteSE3Curve::setMinSamplingPeriod ( Time  time)
virtual

set the minimum sampling period

Implements curves::SE3Curve.

void curves::SemiDiscreteSE3Curve::setSamplingRatio ( const int  ratio)
virtual

Set the sampling ratio. eg. 4 will add a coefficient every 4 extend.

Implements curves::SE3Curve.

virtual void curves::SemiDiscreteSE3Curve::setTimeRange ( Time  minTime,
Time  maxTime 
)
virtual
int curves::SemiDiscreteSE3Curve::size ( ) const
virtual

Implements curves::SE3Curve.

void curves::SemiDiscreteSE3Curve::transformCurve ( const ValueType  T)
virtual

Perform a rigid transformation on the left side of the curve.

Implements curves::Curve< SE3Config >.

virtual void curves::SemiDiscreteSE3Curve::updateFromGTSAMValues ( const gtsam::Values &  values)
virtual

Friends And Related Function Documentation

friend class SamplingPolicy
friend

Definition at line 22 of file SemiDiscreteSE3Curve.hpp.

Definition at line 21 of file SemiDiscreteSE3Curve.hpp.

Member Data Documentation

SamplingPolicy curves::SemiDiscreteSE3Curve::discretePolicy_
private

Definition at line 206 of file SemiDiscreteSE3Curve.hpp.

LocalSupport2CoefficientManager<Coefficient> curves::SemiDiscreteSE3Curve::manager_
private

Definition at line 202 of file SemiDiscreteSE3Curve.hpp.


The documentation for this class was generated from the following file: