GalileanPreintegration.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/geometry/Gal3.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
 
namespace gtsam {

class GTSAM_EXPORT GalileanPreintegration : public PreintegrationBase {
 public:
  using Base = PreintegrationBase;
  using Params = PreintegrationBase::Params;
  using Matrix10 = Eigen::Matrix<double, 10, 10>;
  using Matrix106 = Eigen::Matrix<double, 10, 6>;
  using Matrix910 = Eigen::Matrix<double, 9, 10>;

  inline static constexpr bool kLegacyUsesLogmap = true;
 
 protected:
  Gal3 preintMatrix_;       
  Matrix106 biasJacobian_;  

  static Vector9 NavStateTangent(const Gal3& galilean,
                                 OptionalJacobian<9, 10> H = {});

  void updateGal3(const Vector3& measuredAcc, const Vector3& measuredOmega,
                  double dt, Matrix10* A, Matrix106* B);

  GalileanPreintegration() { resetIntegration(); }
 
 public:
  explicit GalileanPreintegration(
      const std::shared_ptr<Params>& p,
      const imuBias::ConstantBias& biasHat = {});
 
  Rot3 deltaRij() const override { return preintMatrix_.rotation(); }
  Vector3 deltaPij() const override { return preintMatrix_.translation(); }
  Vector3 deltaVij() const override { return preintMatrix_.velocity(); }
  NavState deltaXij() const override {
    return NavState(preintMatrix_.rotation(), preintMatrix_.translation(),
                    preintMatrix_.velocity());
  }

  Vector9 preintegrated() const { return biasCorrectedDelta(biasHat_); }

  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
                             OptionalJacobian<9, 6> H = {}) const override;

  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
              double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;

  void resetIntegration() override;
 
  void print(const std::string& s = "GalileanPreintegration") const override;
  bool equals(const GalileanPreintegration& other, double tol = 1e-9) const;
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& archive, const unsigned int /*version*/) {
    archive& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
    archive& BOOST_SERIALIZATION_NVP(preintMatrix_);
    archive& BOOST_SERIALIZATION_NVP(biasJacobian_);
  }
#endif
};
 
}  // namespace gtsam