gtsam 4.2.0
gtsam
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
23namespace gtsam {
24
29class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
30 double dt_;
31
32 public:
34
35 public:
36 ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
37 : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
38 ~ConstantVelocityFactor() override{};
39
50 gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
51 boost::optional<gtsam::Matrix &> H1 = boost::none,
52 boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
53 // only used to use update() below
54 static const Vector3 b_accel{0.0, 0.0, 0.0};
55 static const Vector3 b_omega{0.0, 0.0, 0.0};
56
57 Matrix99 predicted_H_x1;
58 NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
59
60 Matrix99 error_H_predicted;
61 Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
62
63 if (H1) {
64 *H1 = error_H_predicted * predicted_H_x1;
65 }
66 return error;
67 }
68};
69
70} // namespace gtsam
Navigation state composing of attitude, position, and velocity.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
Binary factor for applying a constant velocity model to a moving body represented as a NavState.
Definition: ConstantVelocityFactor.h:29
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const override
Caclulate error: (x2 - x1.update(dt))) where X1 and X1 are NavStates and dt is the time difference in...
Definition: ConstantVelocityFactor.h:50
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
localCoordinates with optional derivatives
Definition: NavState.cpp:135
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
Integrate forward in time given angular velocity and acceleration in body frame Uses second order int...
Definition: NavState.cpp:171
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:138
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400