AHRSFactor.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
 
/* GTSAM includes */
#include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
#include <iostream>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/version.hpp>
#endif
 
namespace gtsam {
 
namespace internal {

template <class PIM>
Rot3 predictAhrs(const PIM& pim, const Rot3& Ri, const Vector3& bias,
                 OptionalJacobian<3, 3> H1 = {},
                 OptionalJacobian<3, 3> H2 = {}) {
  Rot3 biasCorrected =
      pim.biascorrectedDeltaRij(bias - pim.biasHat(), H2);
 
  // The common case needs no Earth-rotation correction. The compose Jacobian
  // with respect to its second argument is identity, so H2 is already final.
  if (!pim.p().omegaCoriolis || pim.p().omegaCoriolis->isZero(0.0))
    return Ri.compose(biasCorrected, H1);
 
  // Exact rotating-frame attitude transition from Brossard et al.:
  // Rj = Exp(-Omega * dt) * Ri * DeltaR.
  const Rot3 gammaRotation =
      Rot3::Expmap(-(*pim.p().omegaCoriolis) * pim.deltaTij());
  Matrix3 D_gammaRi_Ri, D_Rj_gammaRi, D_Rj_delta;
  const Rot3 gammaRi =
      gammaRotation.compose(Ri, {}, H1 ? &D_gammaRi_Ri : nullptr);
  const Rot3 Rj = gammaRi.compose(biasCorrected, H1 ? &D_Rj_gammaRi : nullptr,
                                  H2 ? &D_Rj_delta : nullptr);
 
  if (H1) *H1 = D_Rj_gammaRi * D_gammaRi_Ri;
  if (H2) *H2 = D_Rj_delta * (*H2);
  return Rj;
}
 
}  // namespace internal

class GTSAM_EXPORT PreintegratedAhrsMeasurements
    : public PreintegratedRotation {
 protected:
  Vector3 biasHat_;  
  Matrix3 preintMeasCov_;  
 
 public:
  PreintegratedAhrsMeasurements() {}

  PreintegratedAhrsMeasurements(const std::shared_ptr<Params>& p,
                                const Vector3& biasHat = Vector3::Zero())
      : PreintegratedRotation(p), biasHat_(biasHat) {
    resetIntegration();
  }

  PreintegratedAhrsMeasurements(const std::shared_ptr<Params>& p,
                                const Vector3& bias_hat, double deltaTij,
                                const Rot3& deltaRij,
                                const Matrix3& delRdelBiasOmega,
                                const Matrix3& preint_meas_cov)
      : PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
        biasHat_(bias_hat),
        preintMeasCov_(preint_meas_cov) {}
 
  Params& p() const { return *std::static_pointer_cast<Params>(p_); }
  const Vector3& biasHat() const { return biasHat_; }
  const Matrix3& preintMeasCov() const { return preintMeasCov_; }

  void print(const std::string& s = "Preintegrated Measurements: ") const;

  bool equals(const PreintegratedAhrsMeasurements& expected,
              double tol = 1e-9) const;

  void resetIntegration();

  void integrateMeasurement(const Vector3& measuredOmega, double deltaT);

  Rot3 predict(const Rot3& Ri, const Vector3& bias,
               gtsam::OptionalJacobian<3, 3> H1 = {},
               gtsam::OptionalJacobian<3, 3> H2 = {}) const {
    return internal::predictAhrs(*this, Ri, bias, H1, H2);
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int version) {
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
    ar& BOOST_SERIALIZATION_NVP(p_);
    ar& BOOST_SERIALIZATION_NVP(biasHat_);
    if (version > 0) {
      ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
    } else if (ARCHIVE::is_loading::value) {
      preintMeasCov_.setZero();
    }
  }
#endif
};

template <class PIM>
class AHRSFactorT
    : public NoiseModelFactorT<Vector3, Rot3, Rot3, Vector3> {
  using This = AHRSFactorT<PIM>;
  using Base = NoiseModelFactorT<Vector3, Rot3, Rot3, Vector3>;
 
  PIM pim_;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename std::shared_ptr<This> shared_ptr;
#else
  typedef std::shared_ptr<This> shared_ptr;
#endif

  AHRSFactorT() = default;

  AHRSFactorT(Key rot_i, Key rot_j, Key bias, const PIM& pim)
      : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov()), rot_i,
             rot_j, bias),
        pim_(pim) {}
 
  ~AHRSFactorT() override = default;

  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 {
    std::cout << s << "AHRSFactor(" << keyFormatter(this->template key<1>())
              << "," << keyFormatter(this->template key<2>()) << ","
              << keyFormatter(this->template key<3>()) << ")\n";
    pim_.print("  preintegrated measurements:");
    this->noiseModel_->print("  noise model: ");
  }

  bool equals(const NonlinearFactor& other, double tol = 1e-9) const override {
    const auto* expected = dynamic_cast<const This*>(&other);
    return expected != nullptr && Base::equals(*expected, tol) &&
           pim_.equals(expected->pim_, tol);
  }

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


  Vector3 evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias,
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3) const override {
    Matrix3 D_predict_Ri, D_predict_bias;
    const Rot3 predictedRj = internal::predictAhrs(
        pim_, Ri, bias, H1 ? &D_predict_Ri : nullptr,
        H3 ? &D_predict_bias : nullptr);
 
    Matrix3 D_error_Rj, D_error_predict;
    const Vector3 error = Rj.logmap(
        predictedRj, H2 ? &D_error_Rj : nullptr,
        H1 || H3 ? &D_error_predict : nullptr);
 
    if (H1) *H1 = D_error_predict * D_predict_Ri;
    if (H2) *H2 = D_error_Rj;
    if (H3) *H3 = D_error_predict * D_predict_bias;
    return error;
  }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor3", boost::serialization::base_object<Base>(*this));
    ar& boost::serialization::make_nvp("_PIM_", pim_);
  }
#endif
};
using AHRSFactor = AHRSFactorT<PreintegratedAhrsMeasurements>;
 
extern template class AHRSFactorT<PreintegratedAhrsMeasurements>;
 
template <>
struct traits<PreintegratedAhrsMeasurements>
    : public Testable<PreintegratedAhrsMeasurements> {};
 
template <class PIM>
struct traits<AHRSFactorT<PIM>> : public Testable<AHRSFactorT<PIM>> {};
 
}  // namespace gtsam
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
BOOST_CLASS_VERSION(gtsam::PreintegratedAhrsMeasurements, 1)
#endif