gtsam  4.1.0
gtsam
VelocityConstraint.h
Go to the documentation of this file.
1 
7 #pragma once
8 
12 
13 namespace gtsam {
14 
15 namespace dynamics {
16 
18 typedef enum {
19  TRAPEZOIDAL, // Constant acceleration
20  EULER_START, // Constant velocity, using starting velocity
21  EULER_END // Constant velocity, using ending velocity
22 } IntegrationMode;
23 
24 }
25 
33 class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> {
34 public:
36 
37 protected:
38 
39  double dt_;
41 
42 public:
43 
48  double dt, double mu = 1000)
49  : Base(noiseModel::Constrained::All(3, mu), key1, key2), dt_(dt), integration_mode_(mode) {}
50 
55  VelocityConstraint(Key key1, Key key2, double dt, double mu = 1000)
56  : Base(noiseModel::Constrained::All(3, mu), key1, key2),
57  dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
58 
63  double dt, const gtsam::SharedNoiseModel& model)
64  : Base(model, key1, key2), dt_(dt), integration_mode_(mode) {}
65 
70  VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel& model)
71  : Base(model, key1, key2), dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
72 
73  virtual ~VelocityConstraint() {}
74 
76  gtsam::NonlinearFactor::shared_ptr clone() const override {
77  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
78  gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
79 
83  gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
84  boost::optional<gtsam::Matrix&> H1=boost::none,
85  boost::optional<gtsam::Matrix&> H2=boost::none) const override {
86  if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
87  boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
88  if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
89  boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
90  return evaluateError_(x1, x2, dt_, integration_mode_);
91  }
92 
93  void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
94  std::string a = "VelocityConstraint: " + s;
95  Base::print(a, formatter);
96  switch(integration_mode_) {
97  case dynamics::TRAPEZOIDAL: std::cout << "Integration: Trapezoidal\n"; break;
98  case dynamics::EULER_START: std::cout << "Integration: Euler (start)\n"; break;
99  case dynamics::EULER_END: std::cout << "Integration: Euler (end)\n"; break;
100  default: std::cout << "Integration: Unknown\n" << std::endl; break;
101  }
102  std::cout << "dt: " << dt_ << std::endl;
103  }
104 
105 private:
106  static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
107  double dt, const dynamics::IntegrationMode& mode) {
108 
109  const Velocity3& v1 = x1.v(), v2 = x2.v();
110  const Point3& p1 = x1.t(), p2 = x2.t();
111  Point3 hx(0,0,0);
112  switch(mode) {
113  case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
114  case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
115  case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
116  default: assert(false); break;
117  }
118  return p2 - hx;
119  }
120 };
121 
122 } // \namespace gtsam
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, const gtsam::SharedNoiseModel &model)
Creates a constraint relating the given variables with arbitrary noise model.
Definition: VelocityConstraint.h:62
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, double mu=1000)
Creates a constraint relating the given variables with fully constrained noise model.
Definition: VelocityConstraint.h:47
gtsam::NoiseModelFactor2< PoseRTV, PoseRTV >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
PoseRTV.h
Pose3 with translational velocity.
NonlinearFactor.h
Non-linear factor base classes.
gtsam::VelocityConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: VelocityConstraint.h:76
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel &model)
Creates a constraint relating the given variables with arbitrary noise model Uses the default Trapezo...
Definition: VelocityConstraint.h:70
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, double dt, double mu=1000)
Creates a constraint relating the given variables with fully constrained noise model Uses the default...
Definition: VelocityConstraint.h:55
gtsam::VelocityConstraint
Constraint to enforce dynamics between the velocities and poses, using a prediction based on a numeri...
Definition: VelocityConstraint.h:33
gtsam::VelocityConstraint::integration_mode_
dynamics::IntegrationMode integration_mode_
time difference between frames in seconds
Definition: VelocityConstraint.h:40
gtsam::dynamics::IntegrationMode
IntegrationMode
controls which model to use for numerical integration to use for constraints
Definition: VelocityConstraint.h:18
gtsam::PoseRTV
Robot state for use with IMU measurements.
Definition: PoseRTV.h:23
gtsam::VelocityConstraint::evaluateError
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const override
Calculates the error for trapezoidal model given.
Definition: VelocityConstraint.h:83
gtsam::VelocityConstraint::print
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Print.
Definition: VelocityConstraint.h:93