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)
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);
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
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::dynamics::EULER_END
@ EULER_END
Definition: VelocityConstraint.h:21
gtsam::VelocityConstraint::integration_mode_
dynamics::IntegrationMode integration_mode_
time difference between frames in seconds
Definition: VelocityConstraint.h:43
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::VelocityConstraint::Base
gtsam::NoiseModelFactor2< PoseRTV, PoseRTV > Base
Definition: VelocityConstraint.h:35
dt
const double dt
Definition: testVelocityConstraint.cpp:15
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, double dt, double mu=1000)
Definition: VelocityConstraint.h:58
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::VelocityConstraint::print
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Definition: VelocityConstraint.h:97
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::VelocityConstraint::dt_
double dt_
Definition: VelocityConstraint.h:42
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, double mu=1000)
Definition: VelocityConstraint.h:50
numericalDerivative.h
Some functions to compute numerical derivatives.
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel &model)
Definition: VelocityConstraint.h:73
gtsam::VelocityConstraint::~VelocityConstraint
~VelocityConstraint() override
Definition: VelocityConstraint.h:76
gtsam::VelocityConstraint::evaluateError_
static gtsam::Vector evaluateError_(const PoseRTV &x1, const PoseRTV &x2, double dt, const dynamics::IntegrationMode &mode)
Definition: VelocityConstraint.h:110
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
mode
static const DiscreteKey mode(modeKey, 2)
gtsam::KeyFormatter
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
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
PoseRTV.h
Pose3 with translational velocity.
gtsam::VelocityConstraint::VelocityConstraint
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, const gtsam::SharedNoiseModel &model)
Definition: VelocityConstraint.h:65
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< PoseRTV, PoseRTV >::key1
Key key1() const
Definition: NonlinearFactor.h:731
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
gtsam::PoseRTV
Definition: PoseRTV.h:23
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< PoseRTV, PoseRTV >::key2
Key key2() const
Definition: NonlinearFactor.h:735
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::dynamics::IntegrationMode
IntegrationMode
Definition: VelocityConstraint.h:18
gtsam
traits
Definition: SFMdata.h:40
gtsam::dynamics::TRAPEZOIDAL
@ TRAPEZOIDAL
Definition: VelocityConstraint.h:19
gtsam::VelocityConstraint
Definition: VelocityConstraint.h:33
v2
Vector v2
Definition: testSerializationBase.cpp:39
gtsam::VelocityConstraint::evaluateError
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: VelocityConstraint.h:86
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::dynamics::EULER_START
@ EULER_START
Definition: VelocityConstraint.h:20
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::VelocityConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: VelocityConstraint.h:79
v1
Vector v1
Definition: testSerializationBase.cpp:38


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:43:10