1/**
2 * @file VelocityConstraint.h
3 * @brief Constraint enforcing the relationship between pose and velocity
4 * @author Alex Cunningham
5 */
6
7#pragma once
8
9#include <gtsam/base/numericalDerivative.h>
10#include <gtsam/nonlinear/NonlinearFactor.h>
11#include <gtsam_unstable/dynamics/PoseRTV.h>
12
13#include <cassert>
14
15namespace gtsam {
16
17namespace dynamics {
18
19/** controls which model to use for numerical integration to use for constraints */
20typedef enum {
21 TRAPEZOIDAL, // Constant acceleration
22 EULER_START, // Constant velocity, using starting velocity
23 EULER_END // Constant velocity, using ending velocity
24} IntegrationMode;
25
26}
27
28/**
29 * Constraint to enforce dynamics between the velocities and poses, using
30 * a prediction based on a numerical integration flag.
31 *
32 * NOTE: this approximation is insufficient for large timesteps, but is accurate
33 * if timesteps are small.
34 */
35class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
36public:
37 typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base;
38
39 // Provide access to the Matrix& version of evaluateError:
40 using Base::evaluateError;
41
42protected:
43
44 double dt_; /// time difference between frames in seconds
45 dynamics::IntegrationMode integration_mode_; ///< Numerical integration control
46
47public:
48
49 /**
50 * Creates a constraint relating the given variables with fully constrained noise model
51 */
52 VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode& mode,
53 double dt, double mu = 1000)
54 : Base(noiseModel::Constrained::All(dim: 3, mu), key1, key2), dt_(dt), integration_mode_(mode) {}
55
56 /**
57 * Creates a constraint relating the given variables with fully constrained noise model
58 * Uses the default Trapezoidal integrator
59 */
60 VelocityConstraint(Key key1, Key key2, double dt, double mu = 1000)
61 : Base(noiseModel::Constrained::All(dim: 3, mu), key1, key2),
62 dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
63
64 /**
65 * Creates a constraint relating the given variables with arbitrary noise model
66 */
67 VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode& mode,
68 double dt, const gtsam::SharedNoiseModel& model)
69 : Base(model, key1, key2), dt_(dt), integration_mode_(mode) {}
70
71 /**
72 * Creates a constraint relating the given variables with arbitrary noise model
73 * Uses the default Trapezoidal integrator
74 */
75 VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel& model)
76 : Base(model, key1, key2), dt_(dt), integration_mode_(dynamics::TRAPEZOIDAL) {}
77
78 ~VelocityConstraint() override {}
79
80 /// @return a deep copy of this factor
81 gtsam::NonlinearFactor::shared_ptr clone() const override {
82 return std::static_pointer_cast<gtsam::NonlinearFactor>(
83 r: gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
84
85 /**
86 * Calculates the error for trapezoidal model given
87 */
88 gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
89 OptionalMatrixType H1, OptionalMatrixType H2) const override {
90 if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
91 h: std::bind(f&: VelocityConstraint::evaluateError_, args: std::placeholders::_1,
92 args: std::placeholders::_2, args: dt_, args: integration_mode_), x1, x2, delta: 1e-5);
93 if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
94 h: std::bind(f&: VelocityConstraint::evaluateError_, args: std::placeholders::_1,
95 args: std::placeholders::_2, args: dt_, args: integration_mode_), x1, x2, delta: 1e-5);
96 return evaluateError_(x1, x2, dt: dt_, mode: integration_mode_);
97 }
98
99 void print(const std::string& s = "", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
100 std::string a = "VelocityConstraint: " + s;
101 Base::print(s: a, keyFormatter: formatter);
102 switch(integration_mode_) {
103 case dynamics::TRAPEZOIDAL: std::cout << "Integration: Trapezoidal\n"; break;
104 case dynamics::EULER_START: std::cout << "Integration: Euler (start)\n"; break;
105 case dynamics::EULER_END: std::cout << "Integration: Euler (end)\n"; break;
106 default: std::cout << "Integration: Unknown\n" << std::endl; break;
107 }
108 std::cout << "dt: " << dt_ << std::endl;
109 }
110
111private:
112 static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
113 double dt, const dynamics::IntegrationMode& mode) {
114
115 const Velocity3& v1 = x1.v(), v2 = x2.v();
116 const Point3& p1 = x1.t(), p2 = x2.t();
117 Point3 hx(0,0,0);
118 switch(mode) {
119 case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
120 case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
121 case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
122 default: assert(false); break;
123 }
124 return p2 - hx;
125 }
126};
127
128} // \namespace gtsam
129