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>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
namespace gtsam {

class ConstantVelocityFactor
    : public NoiseModelFactorT<Vector9, NavState, NavState> {
    double dt_;
 
   public:
    using Base = NoiseModelFactorT<Vector9, NavState, NavState>;
 
    // Provide access to the Matrix& version of evaluateError:
    using Base::evaluateError;
 
   public:
    ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
        : Base(model, i, j), dt_(dt) {}
    ~ConstantVelocityFactor() override {}

    Vector9 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