PlanarGyroFactor.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/Vector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/slam/BetweenFactor.h>
 
#include <optional>
 
#include "gtsam/dllexport.h"
 
namespace gtsam {
using noiseModel::Diagonal;
class GTSAM_EXPORT PlanarGyroParams {
 private:
  const double arw;
  const double biasInstability;
 
 public:
  PlanarGyroParams(double arw, double biasInstability)
      : arw(arw), biasInstability(biasInstability) {}
  double arwSigma(double deltaT);
  double biasInstabilitySigma();
  bool operator==(const PlanarGyroParams& other) const;
  void print(const std::string& s) const;
};

class GTSAM_EXPORT PlanarGyroBiasFactor : public BetweenFactor<double> {
  typedef BetweenFactor<double> Base;
 
 public:
  PlanarGyroBiasFactor(Key bias_i, Key bias_j,
                       const std::shared_ptr<PlanarGyroParams>& p);
  ~PlanarGyroBiasFactor() override {}
};

class GTSAM_EXPORT PlanarGyroFactor
    : public NoiseModelFactorT<Vector3, Pose2, Pose2, double> {
  typedef NoiseModelFactorT<Vector3, Pose2, Pose2, double> Base;
 
 private:
  const std::shared_ptr<PlanarGyroParams> p_;
  const Rot2 deltaR_;
  const double deltaT_;
 
  PlanarGyroFactor(Key pose_i, Key pose_j, Key bias,
                   const std::shared_ptr<PlanarGyroParams>& p, Rot2 dr,
                   double dt);
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  static inline PlanarGyroFactor FromRotation(
      Key pose_i, Key pose_j, Key bias,
      const std::shared_ptr<PlanarGyroParams>& p, Rot2 dr, double dt) {
    return PlanarGyroFactor(pose_i, pose_j, bias, p, dr, dt);
  }

  static inline PlanarGyroFactor FromRate(
      Key pose_i, Key pose_j, Key bias,
      const std::shared_ptr<PlanarGyroParams>& p, double omega, double dt) {
    return PlanarGyroFactor(pose_i, pose_j, bias, p,
                            Rot2::fromAngle(omega * dt), dt);
  }
 
  ~PlanarGyroFactor() override {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override;
  void print(const std::string& s, const KeyFormatter& keyFormatter =
                                       DefaultKeyFormatter) const override;
  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;

  Rot2 deltaR(double bias, OptionalJacobian<1, 1> H = {}) const;

  Rot2 predict(const Rot2& Ri, double bias, OptionalJacobian<1, 1> H1 = {},
               OptionalJacobian<1, 1> H2 = {}) const;

  double computeError(const Rot2& Ri, const Rot2& Rj, double bias,
                      OptionalJacobian<1, 1> H1 = {},
                      OptionalJacobian<1, 1> H2 = {},
                      OptionalJacobian<1, 1> H3 = {}) const;

  Vector3 evaluateError(const Pose2& Pi, const Pose2& Pj, const double& bias,
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3) const override;
};
}  // namespace gtsam