gtsam  4.1.0
gtsam
VelocityConstraint3.h
Go to the documentation of this file.
1 
7 #pragma once
8 
10 
11 namespace gtsam {
12 
13 class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
14 public:
15 
16 protected:
18 
21 
22  double dt_;
23 
24 public:
25 
26  typedef boost::shared_ptr<VelocityConstraint3 > shared_ptr;
27 
29  VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
30  : Base(noiseModel::Constrained::All(1, std::abs(mu)), key1, key2, velKey), dt_(dt) {}
31  virtual ~VelocityConstraint3() {}
32 
34  gtsam::NonlinearFactor::shared_ptr clone() const override {
35  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
36  gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
37 
39  Vector evaluateError(const double& x1, const double& x2, const double& v,
40  boost::optional<Matrix&> H1 = boost::none,
41  boost::optional<Matrix&> H2 = boost::none,
42  boost::optional<Matrix&> H3 = boost::none) const override {
43  const size_t p = 1;
44  if (H1) *H1 = Matrix::Identity(p,p);
45  if (H2) *H2 = -Matrix::Identity(p,p);
46  if (H3) *H3 = Matrix::Identity(p,p)*dt_;
47  return (Vector(1) << x1+v*dt_-x2).finished();
48  }
49 
50 private:
51 
54  template<class ARCHIVE>
55  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
56  ar & boost::serialization::make_nvp("NoiseModelFactor3",
57  boost::serialization::base_object<Base>(*this));
58  }
59 }; // \VelocityConstraint3
60 
61 }
gtsam::VelocityConstraint3::access
friend class boost::serialization::access
Serialization function.
Definition: VelocityConstraint3.h:53
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
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::VelocityConstraint3
Definition: VelocityConstraint3.h:13
gtsam::NoiseModelFactor3< double, double, double >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
NonlinearFactor.h
Non-linear factor base classes.
gtsam::VelocityConstraint3::VelocityConstraint3
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu=1000.0)
TODO: comment.
Definition: VelocityConstraint3.h:29
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::VelocityConstraint3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: VelocityConstraint3.h:34
gtsam::VelocityConstraint3::evaluateError
Vector evaluateError(const double &x1, const double &x2, const double &v, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
x1 + v*dt - x2 = 0, with optional derivatives
Definition: VelocityConstraint3.h:39
gtsam::VelocityConstraint3::VelocityConstraint3
VelocityConstraint3()
default constructor to allow for serialization
Definition: VelocityConstraint3.h:20