PreintegratedRotation.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/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/std_optional_serialization.h>

namespace gtsam {

struct GTSAM_EXPORT PreintegratedRotationParams {
  Matrix3 gyroscopeCovariance;
  std::optional<Vector3> omegaCoriolis;  
  std::optional<Pose3> body_P_sensor;    

  PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}

  PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
                              std::optional<Vector3> omega_coriolis)
    : gyroscopeCovariance(gyroscope_covariance) {
      if (omega_coriolis) {
        omegaCoriolis = *omega_coriolis;
      }
  }

  virtual ~PreintegratedRotationParams() {}

  virtual void print(const std::string& s) const;
  virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;

  void setGyroscopeCovariance(const Matrix3& cov)   { gyroscopeCovariance = cov;  }
  void setOmegaCoriolis(const Vector3& omega)       { omegaCoriolis = omega; }
  void setBodyPSensor(const Pose3& pose)            { body_P_sensor = pose;  }

  const Matrix3& getGyroscopeCovariance()     const { return gyroscopeCovariance; }
  std::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
  std::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }

 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
    ar & BOOST_SERIALIZATION_NVP(body_P_sensor);

    // Provide support for Eigen::Matrix in std::optional
    bool omegaCoriolisFlag = omegaCoriolis.has_value();
    ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
    if (omegaCoriolisFlag) {
      ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
    }
  }
#endif

#ifdef GTSAM_USE_QUATERNIONS
  // Align if we are using Quaternions
public:
    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

class GTSAM_EXPORT PreintegratedRotation {
 public:
  typedef PreintegratedRotationParams Params;

 protected:
  std::shared_ptr<Params> p_;

  double deltaTij_;           
  Rot3 deltaRij_;             
  Matrix3 delRdelBiasOmega_;  

  PreintegratedRotation() {}

 public:

  explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
    resetIntegration();
  }

  PreintegratedRotation(const std::shared_ptr<Params>& p,
                        double deltaTij, const Rot3& deltaRij,
                        const Matrix3& delRdelBiasOmega)
      : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}



  void resetIntegration();

  bool matchesParamsWith(const PreintegratedRotation& other) const {
    return p_ == other.p_;
  }

  const std::shared_ptr<Params>& params() const {
    return p_;
  }
  const double& deltaTij() const {
    return deltaTij_;
  }
  const Rot3& deltaRij() const {
    return deltaRij_;
  }
  const Matrix3& delRdelBiasOmega() const {
    return delRdelBiasOmega_;
  }

  void print(const std::string& s) const;
  bool equals(const PreintegratedRotation& other, double tol) const;


  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
                           OptionalJacobian<3, 3> D_incrR_integratedOmega) const;

  void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
                            OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
                            OptionalJacobian<3, 3> F = {});

  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
                             OptionalJacobian<3, 3> H = {}) const;

  Vector3 integrateCoriolis(const Rot3& rot_i) const;


 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {  // NOLINT
    ar& BOOST_SERIALIZATION_NVP(p_);
    ar& BOOST_SERIALIZATION_NVP(deltaTij_);
    ar& BOOST_SERIALIZATION_NVP(deltaRij_);
    ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
  }
#endif

#ifdef GTSAM_USE_QUATERNIONS
  // Align if we are using Quaternions
  public:
      GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};

}