73 lines
2.3 KiB
C++
73 lines
2.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file ConstantVelocityFactor.h
|
|
* @brief Maintain a constant velocity motion model between two NavStates
|
|
* @author Asa Hammond
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/navigation/NavState.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Binary factor for applying a constant velocity model to a moving body represented as a NavState.
|
|
* The only measurement is dt, the time delta between the states.
|
|
*/
|
|
class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
|
|
double dt_;
|
|
|
|
public:
|
|
using Base = NoiseModelFactor2<NavState, NavState>;
|
|
|
|
// Provide access to the Matrix& version of evaluateError:
|
|
using Base::evaluateError;
|
|
|
|
public:
|
|
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
|
|
: NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
|
|
~ConstantVelocityFactor() override {}
|
|
|
|
/**
|
|
* @brief Calculate error: (x2 - x1.update(dt)))
|
|
* where X1 and X1 are NavStates and dt is
|
|
* the time difference in seconds between the states.
|
|
* @param x1 NavState for key a
|
|
* @param x2 NavState for key b
|
|
* @param H1 optional jacobian in x1
|
|
* @param H2 optional jacobian in x2
|
|
* @return * Vector
|
|
*/
|
|
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
|
|
OptionalMatrixType H1, OptionalMatrixType H2) 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
|