ConstantVelocityFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
22 
23 namespace gtsam {
24 
29 class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
30  double dt_;
31 
32  public:
33  using Base = NoiseModelFactor2<NavState, NavState>;
34 
35  // Provide access to the Matrix& version of evaluateError:
36  using Base::evaluateError;
37 
38  public:
40  : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
42 
54  OptionalMatrixType H1, OptionalMatrixType H2) const override {
55  // only used to use update() below
56  static const Vector3 b_accel{0.0, 0.0, 0.0};
57  static const Vector3 b_omega{0.0, 0.0, 0.0};
58 
59  Matrix99 predicted_H_x1;
60  NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
61 
62  Matrix99 error_H_predicted;
63  Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
64 
65  if (H1) {
66  *H1 = error_H_predicted * predicted_H_x1;
67  }
68  return error;
69  }
70 };
71 
72 } // namespace gtsam
Eigen::Vector3d Vector3
Definition: Vector.h:43
NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
Definition: NavState.cpp:169
noiseModel::Diagonal::shared_ptr model
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
const double dt
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Caclulate error: (x2 - x1.update(dt))) where X1 and X1 are NavStates and dt is the time difference in...
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
traits
Definition: chartTesting.h:28
double error(const Values &c) const override
Non-linear factor base classes.
Pose3 x1
Definition: testPose3.cpp:663
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Definition: NavState.cpp:133
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:03