PreintegrationBase.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/navigation/PreintegrationParams.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
 
#include <iosfwd>
#include <optional>
#include <stdexcept>
#include <string>
#include <utility>
 
namespace gtsam {

class GTSAM_EXPORT PreintegrationBase {
 public:
  typedef imuBias::ConstantBias Bias;
  typedef PreintegrationParams Params;

  inline static constexpr bool kLegacyUsesLogmap = false;
 
 protected:
  std::shared_ptr<Params> p_;

  Bias biasHat_;

  double deltaTij_;

  PreintegrationBase() {}

  virtual ~PreintegrationBase() {}
 
 public:

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


  virtual void resetIntegration() = 0;

  void resetIntegrationAndSetBias(const Bias& biasHat);

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

  const std::shared_ptr<Params>& params() const {
    return p_;
  }

  Params& p() const {
    return *p_;
  }


  const imuBias::ConstantBias& biasHat() const { return biasHat_; }
  double deltaTij() const { return deltaTij_; }
 
  virtual Vector3  deltaPij() const = 0;
  virtual Vector3  deltaVij() const = 0;
  virtual Rot3     deltaRij() const = 0;
  virtual NavState deltaXij() const = 0;

  virtual Vector3 so3TangentAt(double t) const;

  Matrix deskewPoints(
      ConstMatrixView points,
      const Vector3& velocity_i = Vector3::Zero()) const;

  Matrix deskewPointsAtTimes(
      ConstMatrixView points, const Vector& times,
      const Vector3& velocity_i = Vector3::Zero()) const;
 
  // Exposed for MATLAB
  Vector6 biasHatVector() const { return biasHat_.vector(); }

  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
  virtual void print(const std::string& s="") const;


  std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
      const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
      OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = {},
      OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = {},
      OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = {}) const;

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

  virtual void integrateMeasurement(const Vector3& measuredAcc,
      const Vector3& measuredOmega, const double dt);

  void integrateMeasurements(const Matrix& measuredAccs,
                             const Matrix& measuredOmegas, const Matrix& dts);

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

  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                   const Vector3& n_gravity,
                   OptionalJacobian<9, 9> H1 = {},
                   OptionalJacobian<9, 6> H2 = {},
                   OptionalJacobian<9, 3> H3 = {}) const;

  NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                   OptionalJacobian<9, 9> H1 = {},
                   OptionalJacobian<9, 6> H2 = {}) const;
 
 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(p_);
    ar & BOOST_SERIALIZATION_NVP(biasHat_);
    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
  }
#endif
};
 
namespace internal {

template <class PIM>
Vector9 preintegrationError(
    const PIM& pim, const NavState& state_i, const NavState& state_j,
    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
    OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {},
    OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}) {
  Matrix9 D_predict_state_i;
  Matrix96 D_predict_bias_i;
  Matrix93 D_predict_gravity;
  const NavState predictedState_j = pim.predict(
      state_i, bias_i, n_gravity, H1 ? &D_predict_state_i : nullptr,
      H3 ? &D_predict_bias_i : nullptr,
      H4 ? &D_predict_gravity : nullptr);
 
  Matrix9 D_error_state_j, D_error_predict;
  Vector9 error;
  bool useLogmap;
  switch (pim.params()->getImuFactorErrorMode()) {
    case ImuFactorErrorMode::Legacy:
      useLogmap = PIM::kLegacyUsesLogmap;
      break;
    case ImuFactorErrorMode::ComponentWise:
      useLogmap = false;
      break;
    case ImuFactorErrorMode::Logmap:
      useLogmap = true;
      break;
    default:
      throw std::invalid_argument("Unknown ImuFactorErrorMode");
  }
 
  if (useLogmap) {
    error = state_j.logmap(predictedState_j,
                           H2 ? &D_error_state_j : nullptr,
                           H1 || H3 || H4 ? &D_error_predict : nullptr);
  } else {
    error = internal::navStateComponentWiseLocalCoordinates(
        state_j, predictedState_j, H2 ? &D_error_state_j : nullptr,
        H1 || H3 || H4 ? &D_error_predict : nullptr);
  }
 
  if (H1) *H1 = D_error_predict * D_predict_state_i;
  if (H2) *H2 = D_error_state_j;
  if (H3) *H3 = D_error_predict * D_predict_bias_i;
  if (H4) *H4 = D_error_predict * D_predict_gravity;
  return error;
}

template <class PIM>
Vector9 preintegrationError(
    const PIM& pim, const NavState& state_i, const NavState& state_j,
    const imuBias::ConstantBias& bias_i,
    OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {},
    OptionalJacobian<9, 6> H3 = {}) {
  return preintegrationError(pim, state_i, state_j, bias_i,
                             pim.params()->n_gravity, H1, H2, H3);
}

template <class PIM>
Vector9 preintegrationErrorAndJacobians(
    const PIM& pim, const Pose3& pose_i, const Vector3& vel_i,
    const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
    OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {},
    OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {},
    OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 3> H6 = {}) {
  const NavState state_i(pose_i, vel_i), state_j(pose_j, vel_j);
 
  Matrix9 D_error_state_i, D_error_state_j;
  const Vector9 error = preintegrationError(
      pim, state_i, state_j, bias_i, n_gravity,
      H1 || H2 ? &D_error_state_i : nullptr,
      H3 || H4 ? &D_error_state_j : nullptr, H5, H6);
 
  // Separate NavState derivatives. Independent velocity variables retract by
  // straight addition rather than the NavState semidirect-product update.
  if (H1) *H1 = D_error_state_i.leftCols<6>();
  if (H2) *H2 = D_error_state_i.rightCols<3>() * state_i.R().transpose();
  if (H3) *H3 = D_error_state_j.leftCols<6>();
  if (H4) *H4 = D_error_state_j.rightCols<3>() * state_j.R().transpose();
  return error;
}

template <class PIM>
Vector9 preintegrationErrorAndJacobians(
    const PIM& pim, const Pose3& pose_i, const Vector3& vel_i,
    const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i,
    OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {},
    OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {},
    OptionalJacobian<9, 6> H5 = {}) {
  return preintegrationErrorAndJacobians(
      pim, pose_i, vel_i, pose_j, vel_j, bias_i,
      pim.params()->n_gravity, H1, H2, H3, H4, H5);
}

template <class GRAVITY>
struct GravityParametrization;
 
template <>
struct GravityParametrization<Unit3> {
  constexpr static int dimension = 2;
  constexpr static bool usesMagnitude = true;
  static Vector3 vector(const Unit3& gravity, double magnitude,
                        OptionalJacobian<3, 2> H = {}) {
    return gravity.scaled(magnitude, H);
  }
};
 
template <>
struct GravityParametrization<Point3> {
  constexpr static int dimension = 3;
  constexpr static bool usesMagnitude = false;
  static Vector3 vector(const Point3& gravity, double /*magnitude*/,
                        OptionalJacobian<3, 3> H = {}) {
    if (H) H->setIdentity();
    return gravity;
  }
};

template <class GRAVITY, class PIM>
double resolveGravityMagnitude(const std::string& factorName, const PIM& pim,
                               const std::optional<double>& gravityMagnitude) {
  if (!GravityParametrization<GRAVITY>::usesMagnitude) {
    if (gravityMagnitude)
      throw std::invalid_argument(
          factorName + ": gravityMagnitude is only used by the Unit3 "
          "parametrization; the Point3 parametrization optimizes the magnitude "
          "as part of the gravity variable - to constrain it, add a "
          "VectorNormFactor<3> on the gravity variable instead");
    return 0.0;
  }
  if (!gravityMagnitude && !pim.params())
    throw std::invalid_argument(
        factorName + ": the preintegrated measurements have no params to take "
        "the default gravityMagnitude from");
  const double magnitude =
      gravityMagnitude ? *gravityMagnitude : pim.params()->n_gravity.norm();
  if (!(magnitude > 0.0))
    throw std::invalid_argument(factorName +
                                ": gravityMagnitude must be positive");
  return magnitude;
}
 
}  // namespace internal
 
}