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
23 
24 }
25 
33 class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
34 public:
35  typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
36 
37  // Provide access to the Matrix& version of evaluateError:
38  using Base::evaluateError;
39 
40 protected:
41 
42  double dt_;
44 
45 public:
46 
51  double dt, double mu = 1000)
52  : Base(noiseModel::Constrained::All(3, mu), key1, key2), dt_(dt), integration_mode_(mode) {}
53 
58  VelocityConstraint(Key key1, Key key2, double dt, double mu = 1000)
59  : Base(noiseModel::Constrained::All(3, mu), key1, key2),
60  dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
61 
66  double dt, const gtsam::SharedNoiseModel& model)
67  : Base(model, key1, key2), dt_(dt), integration_mode_(mode) {}
68 
74  : Base(model, key1, key2), dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
75 
76  ~VelocityConstraint() override {}
77 
80  return std::static_pointer_cast<gtsam::NonlinearFactor>(
82 
87  OptionalMatrixType H1, OptionalMatrixType H2) const override {
88  if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
89  std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1,
90  std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
91  if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
92  std::bind(VelocityConstraint::evaluateError_, std::placeholders::_1,
93  std::placeholders::_2, dt_, integration_mode_), x1, x2, 1e-5);
94  return evaluateError_(x1, x2, dt_, integration_mode_);
95  }
96 
97  void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
98  std::string a = "VelocityConstraint: " + s;
100  switch(integration_mode_) {
101  case dynamics::TRAPEZOIDAL: std::cout << "Integration: Trapezoidal\n"; break;
102  case dynamics::EULER_START: std::cout << "Integration: Euler (start)\n"; break;
103  case dynamics::EULER_END: std::cout << "Integration: Euler (end)\n"; break;
104  default: std::cout << "Integration: Unknown\n" << std::endl; break;
105  }
106  std::cout << "dt: " << dt_ << std::endl;
107  }
108 
109 private:
111  double dt, const dynamics::IntegrationMode& mode) {
112 
113  const Velocity3& v1 = x1.v(), v2 = x2.v();
114  const Point3& p1 = x1.t(), p2 = x2.t();
115  Point3 hx(0,0,0);
116  switch(mode) {
117  case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
118  case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
119  case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
120  default: assert(false); break;
121  }
122  return p2 - hx;
123  }
124 };
125 
126 } // \namespace gtsam
Vector v2
VelocityConstraint(Key key1, Key key2, double dt, double mu=1000)
Vector v1
Vector3f p1
noiseModel::Diagonal::shared_ptr model
VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel &model)
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
double mu
Some functions to compute numerical derivatives.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
gtsam::NoiseModelFactor2< PoseRTV, PoseRTV > Base
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
static gtsam::Vector evaluateError_(const PoseRTV &x1, const PoseRTV &x2, double dt, const dynamics::IntegrationMode &mode)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
const KeyFormatter & formatter
const double dt
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, const gtsam::SharedNoiseModel &model)
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
const Symbol key1('v', 1)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
std::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
Pose3 with translational velocity.
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
Pose3 x1
Definition: testPose3.cpp:663
std::shared_ptr< This > shared_ptr
Vector evaluateError_(const PointReferenceFrameFactor &c, const Point2 &global, const Pose2 &trans, const Point2 &local)
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, double mu=1000)
const Point3 & t() const
Definition: PoseRTV.h:58
static Point3 p2
static const DiscreteKey mode(modeKey, 2)
Vector3 Point3
Definition: Point3.h:38
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
dynamics::IntegrationMode integration_mode_
time difference between frames in seconds
const Velocity3 & v() const
Definition: PoseRTV.h:57
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
const Symbol key2('v', 2)
gtsam::NonlinearFactor::shared_ptr clone() const override


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:44