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 = 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 {}

    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