VelocityConstraint3.h
Go to the documentation of this file.
1 
7 #pragma once
8 
10 
11 namespace gtsam {
12 
13 class VelocityConstraint3 : public NoiseModelFactorN<double, double, double> {
14 public:
15 
16 protected:
18 
21 
22  double dt_;
23 
24 public:
25 
26  // Provide access to the Matrix& version of evaluateError:
27  using Base::evaluateError;
28 
29  typedef std::shared_ptr<VelocityConstraint3 > shared_ptr;
30 
32  VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
33  : Base(noiseModel::Constrained::All(1, std::abs(mu)), key1, key2, velKey), dt_(dt) {}
34  ~VelocityConstraint3() override {}
35 
38  return std::static_pointer_cast<gtsam::NonlinearFactor>(
40 
42  Vector evaluateError(const double& x1, const double& x2, const double& v,
44  OptionalMatrixType H3) const override {
45  const size_t p = 1;
46  if (H1) *H1 = Matrix::Identity(p,p);
47  if (H2) *H2 = -Matrix::Identity(p,p);
48  if (H3) *H3 = Matrix::Identity(p,p)*dt_;
49  return (Vector(1) << x1+v*dt_-x2).finished();
50  }
51 
52 private:
53 
54 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
55 
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("NoiseModelFactor3",
61  boost::serialization::base_object<Base>(*this));
62  }
63 #endif
64 }; // \VelocityConstraint3
65 
66 }
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::NoiseModelFactorN< double, double, double >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::VelocityConstraint3::shared_ptr
std::shared_ptr< VelocityConstraint3 > shared_ptr
Definition: VelocityConstraint3.h:29
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::VelocityConstraint3::VelocityConstraint3
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu=1000.0)
TODO: comment.
Definition: VelocityConstraint3.h:32
gtsam::VelocityConstraint3::evaluateError
Vector evaluateError(const double &x1, const double &x2, const double &v, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Definition: VelocityConstraint3.h:42
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:246
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::VelocityConstraint3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: VelocityConstraint3.h:37
gtsam::VelocityConstraint3::~VelocityConstraint3
~VelocityConstraint3() override
Definition: VelocityConstraint3.h:34
gtsam::NoiseModelFactorN< double, double, double >::key1
Key key1() const
Definition: NonlinearFactor.h:732
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< double, double, double >::key2
Key key2() const
Definition: NonlinearFactor.h:736
gtsam
traits
Definition: SFMdata.h:40
gtsam::VelocityConstraint3::Base
NoiseModelFactorN< double, double, double > Base
Definition: VelocityConstraint3.h:17
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::VelocityConstraint3::dt_
double dt_
Definition: VelocityConstraint3.h:22
gtsam::VelocityConstraint3
Definition: VelocityConstraint3.h:13
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::VelocityConstraint3::VelocityConstraint3
VelocityConstraint3()
Definition: VelocityConstraint3.h:20
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:18:24