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/navigation/PreintegratedRotation.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
 
namespace gtsam {
 
class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
 
 protected:
 
  Vector3 biasHat_; 
  Matrix3 preintMeasCov_; 
 
  friend class AHRSFactor;
 
 public:
 
  PreintegratedAhrsMeasurements() {}
 
  PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
      const Vector3& biasHat) :
      PreintegratedRotation(p), biasHat_(biasHat) {
    resetIntegration();
  }
 
  PreintegratedAhrsMeasurements(
      const boost::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 *boost::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&, double tol = 1e-9) const;
 
  void resetIntegration();
 
  void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
 
  Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
 
  // This function is only used for test purposes
  // (compare numerical derivatives wrt analytic ones)
  static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
      const Vector3& delta_angles);
 
  PreintegratedAhrsMeasurements(const Vector3& biasHat,
                                const Matrix3& measuredOmegaCovariance)
      : PreintegratedRotation(boost::make_shared<Params>()),
        biasHat_(biasHat) {
    p_->gyroscopeCovariance = measuredOmegaCovariance;
    resetIntegration();
  }
 
private:
 
  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_);
  }
};
 
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
 
  typedef AHRSFactor This;
  typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
 
  PreintegratedAhrsMeasurements _PIM_;
 
  AHRSFactor() {}
 
public:
 
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
  typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
#else
  typedef boost::shared_ptr<AHRSFactor> shared_ptr;
#endif
 
  AHRSFactor(Key rot_i, Key rot_j, Key bias,
      const PreintegratedAhrsMeasurements& preintegratedMeasurements);
 
  ~AHRSFactor() override {
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
  void print(const std::string& s, const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const override;
 
  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
 
  const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
    return _PIM_;
  }
 
  Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
      const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
          boost::none) const override;
 
  static Rot3 Predict(
      const Rot3& rot_i, const Vector3& bias,
      const PreintegratedAhrsMeasurements preintegratedMeasurements);
 
  typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
 
  AHRSFactor(Key rot_i, Key rot_j, Key bias,
      const PreintegratedMeasurements& preintegratedMeasurements,
      const Vector3& omegaCoriolis,
      const boost::optional<Pose3>& body_P_sensor = boost::none);
 
  static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
      const PreintegratedMeasurements preintegratedMeasurements,
      const Vector3& omegaCoriolis,
      const boost::optional<Pose3>& body_P_sensor = boost::none);
 
private:
 
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar
        & boost::serialization::make_nvp("NoiseModelFactor3",
            boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(_PIM_);
  }
 
};
// AHRSFactor
 
} //namespace gtsam