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/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/PreintegrationCombinedParams.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsam {

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

  PreintegrationCombinedParams()
      : biasAccCovariance(I_3x3),
        biasOmegaCovariance(I_3x3),
        biasAccOmegaInt(I_6x6) {}

  PreintegrationCombinedParams(const Vector3& n_gravity_)
      : PreintegrationParams(n_gravity_),
        biasAccCovariance(I_3x3),
        biasOmegaCovariance(I_3x3),
        biasAccOmegaInt(I_6x6) {}

  // 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; }
  void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; }

  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
  const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }

 private:
#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(PreintegrationParams);
    ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
    ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
    ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
  }
#endif

 public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

}  // namespace gtsam