1/**
2 * @file VelocityConstraint3.h
3 * @brief A simple 3-way factor constraining double poses and velocity
4 * @author Duy-Nguyen Ta
5 */
6
7#pragma once
8
9#include <gtsam/nonlinear/NonlinearFactor.h>
10
11namespace gtsam {
12
13class VelocityConstraint3 : public NoiseModelFactorN<double, double, double> {
14public:
15
16protected:
17 typedef NoiseModelFactorN<double, double, double> Base;
18
19 /** default constructor to allow for serialization */
20 VelocityConstraint3() {}
21
22 double dt_;
23
24public:
25
26 // Provide access to the Matrix& version of evaluateError:
27 using Base::evaluateError;
28
29 typedef std::shared_ptr<VelocityConstraint3 > shared_ptr;
30
31 ///TODO: comment
32 VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
33 : Base(noiseModel::Constrained::All(dim: 1, mu: std::abs(x: mu)), key1, key2, velKey), dt_(dt) {}
34 ~VelocityConstraint3() override {}
35
36 /// @return a deep copy of this factor
37 gtsam::NonlinearFactor::shared_ptr clone() const override {
38 return std::static_pointer_cast<gtsam::NonlinearFactor>(
39 r: gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
40
41 /** x1 + v*dt - x2 = 0, with optional derivatives */
42 Vector evaluateError(const double& x1, const double& x2, const double& v,
43 OptionalMatrixType H1, OptionalMatrixType H2,
44 OptionalMatrixType H3) const override {
45 const size_t p = 1;
46 if (H1) *H1 = Matrix::Identity(rows: p,cols: p);
47 if (H2) *H2 = -Matrix::Identity(rows: p,cols: p);
48 if (H3) *H3 = Matrix::Identity(rows: p,cols: p)*dt_;
49 return (Vector(1) << x1+v*dt_-x2).finished();
50 }
51
52private:
53
54#if GTSAM_ENABLE_BOOST_SERIALIZATION
55 /** Serialization function */
56 friend class boost::serialization::access;
57 template<class ARCHIVE>
58 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
59 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
60 ar & boost::serialization::make_nvp(n: "NoiseModelFactor3",
61 v&: boost::serialization::base_object<Base>(d&: *this));
62 }
63#endif
64}; // \VelocityConstraint3
65
66}
67