Loading...
Searching...
No Matches
ConstantVelocityFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
double dt_;
public:
using Base = NoiseModelFactorN<NavState, NavState>;
public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
: NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
~ConstantVelocityFactor() override{};
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 {
// only used to use update() below
static const Vector3 b_accel{0.0, 0.0, 0.0};
static const Vector3 b_omega{0.0, 0.0, 0.0};
Matrix99 predicted_H_x1;
NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
Matrix99 error_H_predicted;
Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
if (H1) {
*H1 = error_H_predicted * predicted_H_x1;
}
return error;
}
};
} // namespace gtsam
NavState.h
Navigation state composing of attitude, position, and velocity.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::ConstantVelocityFactor::evaluateError
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
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::NavState::localCoordinates
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
gtsam::NavState::update
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
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< NavState, NavState >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469