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>
 
namespace gtsam {
 
struct GTSAM_EXPORT PreintegratedRotationParams {
  Matrix3 gyroscopeCovariance;
  boost::optional<Vector3> omegaCoriolis;  
  boost::optional<Pose3> body_P_sensor;    
 
  PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
 
  PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
                              boost::optional<Vector3> omega_coriolis)
    : gyroscopeCovariance(gyroscope_covariance) {
      if (omega_coriolis)
        omegaCoriolis.reset(omega_coriolis.get());
  }
 
  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.reset(omega); }
  void setBodyPSensor(const Pose3& pose)            { body_P_sensor.reset(pose);  }
 
  const Matrix3& getGyroscopeCovariance()     const { return gyroscopeCovariance; }
  boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
  boost::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
 
 private:
  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 boost::optional
    bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
    ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
    if (omegaCoriolisFlag) {
      ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
    }
  }
 
#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:
  boost::shared_ptr<Params> p_;
 
  double deltaTij_;           
  Rot3 deltaRij_;             
  Matrix3 delRdelBiasOmega_;  
 
  PreintegratedRotation() {}
 
 public:
 
  explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
    resetIntegration();
  }
 
  PreintegratedRotation(const boost::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 boost::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 = boost::none,
                            OptionalJacobian<3, 3> F = boost::none);
 
  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
                             OptionalJacobian<3, 3> H = boost::none) const;
 
  Vector3 integrateCoriolis(const Rot3& rot_i) const;
 
 
 private:
  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_);
  }
 
#ifdef GTSAM_USE_QUATERNIONS
  // Align if we are using Quaternions
  public:
          GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
 
template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
 
}