PreintegrationCombinedParams.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
 
/* GTSAM includes */
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {

struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
  Matrix3 biasAccCovariance;    
  Matrix3 biasOmegaCovariance;  

  PreintegrationCombinedParams()
    : biasAccCovariance(I_3x3),
    biasOmegaCovariance(I_3x3) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
    biasAccOmegaInt.setZero();
#endif
  }

  PreintegrationCombinedParams(const Vector3& n_gravity_)
    : PreintegrationParams(n_gravity_),
    biasAccCovariance(I_3x3),
    biasOmegaCovariance(I_3x3) {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
    biasAccOmegaInt.setZero();
#endif
  }
 
  // Default Params for a Z-down navigation frame, such as NED: gravity points
  // along positive Z-axis
  static std::shared_ptr<PreintegrationCombinedParams> MakeSharedD(
      double g = 9.81) {
    return std::shared_ptr<PreintegrationCombinedParams>(
        new PreintegrationCombinedParams(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<PreintegrationCombinedParams> MakeSharedU(
      double g = 9.81) {
    return std::shared_ptr<PreintegrationCombinedParams>(
        new PreintegrationCombinedParams(Vector3(0, 0, -g)));
  }
 
  void print(const std::string& s = "") const override;
  bool equals(const PreintegratedRotationParams& other,
              double tol) const override;
 
  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; }
  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; }
 
  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
  
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  Matrix6 biasAccOmegaInt;
  void setBiasAccOmegaInit(const Matrix6& cov) {
    std::cerr << "Warning: setBiasAccOmegaInit() is deprecated and no longer used." << std::endl;
    biasAccOmegaInt = cov;
  }
  const Matrix6& getBiasAccOmegaInit() const {
    std::cerr << "Warning: getBiasAccOmegaInit() is deprecated and no longer used." << std::endl;
    return biasAccOmegaInt;
  }
#endif
 
 private:
#if 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(PreintegrationParams);
    ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
    ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
    ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
#endif
  }
#endif
};
 
}  // namespace gtsam