CombinedImuFactorWithGravity.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/CombinedImuFactor.h>
 
namespace gtsam {

template <class PIM = PreintegratedCombinedMeasurements, class GRAVITY = Unit3>
class GTSAM_EXPORT CombinedImuFactorWithGravityT
    : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                               imuBias::ConstantBias, imuBias::ConstantBias,
                               GRAVITY> {
 private:
  typedef CombinedImuFactorWithGravityT<PIM, GRAVITY> This;
  typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
                            imuBias::ConstantBias, 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;

  CombinedImuFactorWithGravityT() : gravityMagnitude_(0.0) {}

  CombinedImuFactorWithGravityT(Key pose_i, Key vel_i, Key pose_j, Key vel_j,
                                Key bias_i, Key bias_j, Key gravity,
                                const PIM& preintegratedMeasurements,
                                std::optional<double> gravityMagnitude = {})
      : Base(noiseModel::Gaussian::Covariance(
                 preintegratedMeasurements.residualCovariance()),
             pose_i, vel_i, pose_j, vel_j, bias_i, bias_j, gravity),
        pim_(preintegratedMeasurements),
        gravityMagnitude_(internal::resolveGravityMagnitude<GRAVITY>(
            "CombinedImuFactorWithGravityT", preintegratedMeasurements,
            gravityMagnitude)) {}
 
  ~CombinedImuFactorWithGravityT() 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 imuBias::ConstantBias& bias_j,
                       const GRAVITY& gravity, OptionalMatrixType H1,
                       OptionalMatrixType H2, OptionalMatrixType H3,
                       OptionalMatrixType H4, OptionalMatrixType H5,
                       OptionalMatrixType H6, OptionalMatrixType H7) 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(
        "NoiseModelFactor7", boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(pim_);
    ar& BOOST_SERIALIZATION_NVP(gravityMagnitude_);
  }
#endif
};
// class CombinedImuFactorWithGravityT

using CombinedImuFactorWithGravityDirection =
    CombinedImuFactorWithGravityT<PreintegratedCombinedMeasurements, Unit3>;

using CombinedImuFactorWithGravityVector =
    CombinedImuFactorWithGravityT<PreintegratedCombinedMeasurements, Point3>;
 
// operator<< for CombinedImuFactorWithGravityT
template <class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream& operator<<(
    std::ostream& os, const CombinedImuFactorWithGravityT<PIM, GRAVITY>& f);
 
template <class PIM, class GRAVITY>
struct traits<CombinedImuFactorWithGravityT<PIM, GRAVITY>>
    : public Testable<CombinedImuFactorWithGravityT<PIM, GRAVITY>> {};
 
}  // namespace gtsam