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>
#include <boost/make_shared.hpp>

namespace gtsam {

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

  PreintegrationParams(const Vector3& n_gravity)
      : 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 boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
    return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g));
  }

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

  void print(const std::string& s) const;
  bool equals(const PreintegratedRotation::Params& other, double tol) const;

  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; }
  bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }

protected:
  PreintegrationParams() {}

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    namespace bs = ::boost::serialization;
    ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
         boost::serialization::base_object<PreintegratedRotationParams>(*this));
    ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
    ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
    ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
    ar & BOOST_SERIALIZATION_NVP(n_gravity);
  }
};

} // namespace gtsam