PreintegrationParams.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/PreintegratedRotation.h>

namespace gtsam {

struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
  Matrix3 accelerometerCovariance;
  Matrix3 integrationCovariance; 
  bool use2ndOrderCoriolis; 
  Vector3 n_gravity; 

  PreintegrationParams()
      : PreintegratedRotationParams(),
        accelerometerCovariance(I_3x3),
        integrationCovariance(I_3x3),
        use2ndOrderCoriolis(false),
        n_gravity(0, 0, -1) {}

  PreintegrationParams(const Vector3& n_gravity_)
      : PreintegratedRotationParams(),
        accelerometerCovariance(I_3x3),
        integrationCovariance(I_3x3),
        use2ndOrderCoriolis(false),
        n_gravity(n_gravity_) {}

  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
  static std::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
    return std::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
  }

  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
  static std::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
    return std::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
  }

  void print(const std::string& s="") const override;
  bool equals(const PreintegratedRotationParams& other, double tol) const override;

  void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
  void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }
  void setUse2ndOrderCoriolis(bool flag)              { use2ndOrderCoriolis = flag; }

  const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
  const Matrix3& getIntegrationCovariance()   const { return integrationCovariance; }
  const Vector3& getGravity()   const { return n_gravity; }
  bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }

protected:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
    ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
    ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
    ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
    ar & BOOST_SERIALIZATION_NVP(n_gravity);
  }
#endif

#ifdef GTSAM_USE_QUATERNIONS
  // Align if we are using Quaternions
public:
        GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

} // namespace gtsam