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/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/geometry/Pose3.h>
 
#include "gtsam/dllexport.h"
 
namespace gtsam {
 
namespace internal {
struct GTSAM_EXPORT IncrementalRotation {
  const Vector3& measuredOmega;
  const double deltaT;
  const std::optional<Pose3>& body_P_sensor;

  Rot3 operator()(const Vector3& bias,
                  OptionalJacobian<3, 3> H_bias = {}) const;
};
 
}  // namespace internal

GTSAM_EXPORT Rot3 integrateSequentialRotations(
    const Vector& times, ConstMatrixView measuredOmegas,
    const Vector3& biasHat = Vector3::Zero(),
    const Rot3& body_R_sensor = Rot3());

GTSAM_EXPORT Rot3 integrateSingleSpeedConing(
    const Vector& times, ConstMatrixView measuredOmegas,
    const Vector3& biasHat = Vector3::Zero(),
    const Rot3& body_R_sensor = Rot3());

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 = {},
                              std::optional<Pose3> body_P_sensor = {})
    : gyroscopeCovariance(gyroscope_covariance),
      omegaCoriolis(omega_coriolis),
      body_P_sensor(body_P_sensor) {}
 
  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:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    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
};

class GTSAM_EXPORT PreintegratedRotation {
 public:
  typedef PreintegratedRotationParams Params;
 
 protected:
  std::shared_ptr<Params> p_;
 
  double deltaTij_;           
  Rot3 deltaRij_;             
  Matrix3 delRdelBiasOmega_;  
 
  public:
    
  PreintegratedRotation() {}

  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) {}



  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;


  void resetIntegration();

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

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


 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  Vector3 integrateCoriolis(const Rot3& wRi,
                            OptionalJacobian<3, 3> H = {}) const;

  inline Rot3 incrementalRotation(
      const Vector3& measuredOmega, const Vector3& bias, double deltaT,
      OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
    internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
    Rot3 incrR = f(bias, D_incrR_integratedOmega);
    // Backwards compatible "weird" Jacobian, no longer used.
    if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
    return incrR;
  }

  void integrateMeasurement(const Vector3& measuredOmega,
                            const Vector3& biasHat, double deltaT,
                            OptionalJacobian<3, 3> D_incrR_integratedOmega,
                            OptionalJacobian<3, 3> F);
 
#endif

 
 private:
#if 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
};
 
template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
 
}