ImuFactorWithGravity.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/ImuFactor.h>
 
namespace gtsam {

template <class PIM = PreintegratedImuMeasurements, class GRAVITY = Unit3>
class GTSAM_EXPORT ImuFactorWithGravityT
    : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                               imuBias::ConstantBias, GRAVITY> {
private:
 
  typedef ImuFactorWithGravityT<PIM, GRAVITY> This;
  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                            imuBias::ConstantBias, GRAVITY> Base;
 
  PIM pim_;
  double gravityMagnitude_;  
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  typedef std::shared_ptr<This> shared_ptr;

  ImuFactorWithGravityT() : gravityMagnitude_(0.0) {}

  ImuFactorWithGravityT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      Key gravity, const PIM& preintegratedMeasurements,
      std::optional<double> gravityMagnitude = {})
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovariance()),
             pose_i, vel_i, pose_j, vel_j, bias, gravity),
        pim_(preintegratedMeasurements),
        gravityMagnitude_(internal::resolveGravityMagnitude<GRAVITY>(
            "ImuFactorWithGravityT", preintegratedMeasurements,
            gravityMagnitude)) {}
 
  ~ImuFactorWithGravityT() override {
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

  const PIM& preintegratedMeasurements() const {
    return pim_;
  }

  double gravityMagnitude() const {
    return gravityMagnitude_;
  }


  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
                       const Pose3& pose_j, const Vector3& vel_j,
                       const imuBias::ConstantBias& bias_i,
                       const GRAVITY& gravity, OptionalMatrixType H1,
                       OptionalMatrixType H2, OptionalMatrixType H3,
                       OptionalMatrixType H4, OptionalMatrixType H5,
                       OptionalMatrixType H6) const override;

  template <typename MethodPIMArg = PIM,
    typename = typename std::enable_if<
        std::is_same<MethodPIMArg, PreintegratedImuMeasurementsT<TangentPreintegration>>::value
    >::type
  >
  static typename ImuFactorWithGravityT<MethodPIMArg, GRAVITY>::shared_ptr Merge(
    const typename ImuFactorWithGravityT<MethodPIMArg, GRAVITY>::shared_ptr& f01,
    const typename ImuFactorWithGravityT<MethodPIMArg, GRAVITY>::shared_ptr& f12
  ) {
    if (f01->template key<5>() != f12->template key<5>())
      throw std::domain_error("ImuFactorWithGravityT::Merge: IMU bias keys must be the same");
 
    if (f01->template key<6>() != f12->template key<6>())
      throw std::domain_error("ImuFactorWithGravityT::Merge: gravity keys must be the same");
 
    if (internal::GravityParametrization<GRAVITY>::usesMagnitude &&
        std::abs(f01->gravityMagnitude() - f12->gravityMagnitude()) > 1e-9)
      throw std::domain_error(
          "ImuFactorWithGravityT::Merge: gravity magnitudes must be the same");
 
    if (f01->template key<3>() != f12->template key<1>() || f01->template key<4>() != f12->template key<2>())
      throw std::domain_error(
          "ImuFactorWithGravityT::Merge: intermediate pose, velocity keys need to match up");
 
    auto pim02 = ImuFactorT<MethodPIMArg>::Merge(f01->preintegratedMeasurements(),
                                                 f12->preintegratedMeasurements());
 
    return std::make_shared<This>(
        f01->template key<1>(),  // P0
        f01->template key<2>(),  // V0
        f12->template key<3>(),  // P2
        f12->template key<4>(),  // V2
        f01->template key<5>(),  // B
        f01->template key<6>(),  // G
        pim02,
        internal::GravityParametrization<GRAVITY>::usesMagnitude
            ? std::optional<double>(f01->gravityMagnitude())
            : std::nullopt);
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // Archive name for the base follows the sibling factors' convention:
    ar & boost::serialization::make_nvp("NoiseModelFactor6",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(pim_);
    ar & BOOST_SERIALIZATION_NVP(gravityMagnitude_);
  }
#endif
};
// class ImuFactorWithGravityT

using ImuFactorWithGravityDirection =
    ImuFactorWithGravityT<PreintegratedImuMeasurements, Unit3>;

using ImuFactorWithGravityVector =
    ImuFactorWithGravityT<PreintegratedImuMeasurements, Point3>;
 
// operator<< for ImuFactorWithGravityT
template <class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os,
                                      const ImuFactorWithGravityT<PIM, GRAVITY>& f);
 
template <class PIM, class GRAVITY>
struct traits<ImuFactorWithGravityT<PIM, GRAVITY>>
    : public Testable<ImuFactorWithGravityT<PIM, GRAVITY>> {};

template <class PIM = PreintegratedImuMeasurements, class GRAVITY = Unit3>
class GTSAM_EXPORT ImuFactor2WithGravityT
    : public NoiseModelFactorT<Vector9, NavState, NavState,
                               imuBias::ConstantBias, GRAVITY> {
private:
 
  typedef ImuFactor2WithGravityT<PIM, GRAVITY> This;
  typedef NoiseModelFactorT<Vector9, NavState, NavState, imuBias::ConstantBias,
                            GRAVITY>
      Base;
 
  PIM pim_;
  double gravityMagnitude_;  
 
public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  ImuFactor2WithGravityT() : gravityMagnitude_(0.0) {}

  ImuFactor2WithGravityT(Key state_i, Key state_j, Key bias, Key gravity,
      const PIM& preintegratedMeasurements,
      std::optional<double> gravityMagnitude = {})
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovariance()),
             state_i, state_j, bias, gravity),
        pim_(preintegratedMeasurements),
        gravityMagnitude_(internal::resolveGravityMagnitude<GRAVITY>(
            "ImuFactor2WithGravityT", preintegratedMeasurements,
            gravityMagnitude)) {}
 
  ~ImuFactor2WithGravityT() override {
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

  const PIM& preintegratedMeasurements() const {
    return pim_;
  }

  double gravityMagnitude() const {
    return gravityMagnitude_;
  }


  Vector9 evaluateError(const NavState& state_i, const NavState& state_j,
                        const imuBias::ConstantBias& bias_i,
                        const GRAVITY& gravity, OptionalMatrixType H1,
                        OptionalMatrixType H2, OptionalMatrixType H3,
                        OptionalMatrixType H4) const override;
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    // Archive name for the base follows the sibling factors' convention:
    ar & boost::serialization::make_nvp("NoiseModelFactor4",
         boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(pim_);
    ar & BOOST_SERIALIZATION_NVP(gravityMagnitude_);
  }
#endif
};
// class ImuFactor2WithGravityT

using ImuFactor2WithGravityDirection =
    ImuFactor2WithGravityT<PreintegratedImuMeasurements, Unit3>;

using ImuFactor2WithGravityVector =
    ImuFactor2WithGravityT<PreintegratedImuMeasurements, Point3>;
 
// operator<< for ImuFactor2WithGravityT
template <class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream& operator<<(std::ostream& os,
                                      const ImuFactor2WithGravityT<PIM, GRAVITY>& f);
 
template <class PIM, class GRAVITY>
struct traits<ImuFactor2WithGravityT<PIM, GRAVITY>>
    : public Testable<ImuFactor2WithGravityT<PIM, GRAVITY>> {};
 
}