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  ~VelocityConstraint3() override {}
32 
35  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
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 }
double mu
ArrayXcf v
Definition: Cwise_arg.cpp:1
Definition: Half.h:150
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
const SharedNoiseModel & noiseModel() const
access to the noise model
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
const double dt
NoiseModelFactor3< double, double, double > Base
Eigen::VectorXd Vector
Definition: Vector.h:38
boost::shared_ptr< VelocityConstraint3 > shared_ptr
boost::shared_ptr< This > shared_ptr
friend class boost::serialization::access
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
Pose3 x1
Definition: testPose3.cpp:588
float * p
gtsam::NonlinearFactor::shared_ptr clone() const override
#define abs(x)
Definition: datatypes.h:17
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu=1000.0)
TODO: comment.
void serialize(ARCHIVE &ar, const unsigned int)


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:24