LieGroupPreintegration.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/navigation/ManifoldPreintegration.h>
 
namespace gtsam {

class GTSAM_EXPORT LieGroupPreintegration : public ManifoldPreintegration {
 protected:
  LieGroupPreintegration() = default;
 
 public:
  inline static constexpr bool kLegacyUsesLogmap = true;

  LieGroupPreintegration(
      const std::shared_ptr<Params>& params,
      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());

  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias,
                             OptionalJacobian<9, 6> H = {}) const override;
 
 protected:
  void updateFactor(const Vector3& bodyAcceleration, const Vector3& bodyOmega,
                    double dt, OptionalJacobian<9, 9> F = {},
                    OptionalJacobian<9, 3> G1 = {},
                    OptionalJacobian<9, 3> G2 = {}) override;
 
  void updateBiasJacobians(const Rot3& oldRotation,
                           const Vector3& bodyAcceleration,
                           const Vector3& bodyOmega, double dt,
                           const Matrix9& stateTransition,
                           const Matrix93& accelerationJacobian,
                           const Matrix93& omegaJacobian) override;
 
 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(ManifoldPreintegration);
  }
#endif
};
 
}  // namespace gtsam