ExtendedPriorFactor.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/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
#include <optional>
 
namespace gtsam {

template <class VALUE>
class ExtendedPriorFactor : public NoiseModelFactorN<VALUE> {
 public:
  typedef VALUE T;

  using NoiseModelFactor1<VALUE>::evaluateError;
 
 protected:
  typedef NoiseModelFactorN<VALUE> Base;
 
  VALUE origin_; 
  std::optional<Vector>
      mean_; 

  GTSAM_CONCEPT_TESTABLE_TYPE(T)
 
 public:
  typedef ExtendedPriorFactor<VALUE> This;


  ExtendedPriorFactor() {}

  ExtendedPriorFactor(Key key, const T& origin, const SharedNoiseModel& model)
      : Base(model, key), origin_(origin) {}

  ExtendedPriorFactor(Key key, const T& origin, const Vector& mean,
                      const SharedNoiseModel& model)
      : Base(model, key), origin_(origin), mean_(mean) {
    if (mean.size() != static_cast<Eigen::Index>(model->dim()))
      throw std::invalid_argument(
          "ExtendedPriorFactor: mean dimension does not match noise model");
  }

  ExtendedPriorFactor(Key key, const T& origin, const Matrix& covariance)
      : Base(noiseModel::Gaussian::Covariance(covariance), key),
        origin_(origin) {}

  ExtendedPriorFactor(Key key, const T& origin, const Vector& mean,
                      const Matrix& covariance)
      : Base(noiseModel::Gaussian::Covariance(covariance), key),
        origin_(origin),
        mean_(mean) {
    if (mean.size() != covariance.rows() ||
        covariance.rows() != covariance.cols())
      throw std::invalid_argument(
          "ExtendedPriorFactor: mean and covariance dimensions do not match");
  }

 
  ~ExtendedPriorFactor() override {}


  void print(const std::string& s, const KeyFormatter& keyFormatter =
                                       DefaultKeyFormatter) const override {
    std::cout << s << "ExtendedPriorFactor on " << keyFormatter(this->key())
              << "\n";
    traits<T>::Print(origin_, "  origin: ");
    if (mean_) {
      gtsam::print(*mean_, "  tangent space mean: ");
    }
    if (this->noiseModel_)
      this->noiseModel_->print("  noise model: ");
    else
      std::cout << "no noise model" << std::endl;
  }

  bool equals(const NonlinearFactor& expected,
              double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&expected);
    if (!e) return false;
    bool mean_equals =
        (!mean_ && !e->mean_) ||
        (mean_ && e->mean_ && equal_with_abs_tol(*mean_, *e->mean_, tol));
    return Base::equals(*e, tol) &&
           traits<T>::Equals(origin_, e->origin_, tol) && mean_equals;
  }


  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  Vector evaluateError(const T& x, OptionalMatrixType H) const override {
    // manifold equivalent of z-x -> Local(x,z)
    Vector error;
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
    if constexpr (internal::HasLocalJacobians<T>::value) {
      if (H) {
        error = -traits<T>::Local(x, origin_, H, OptionalNone);
        *H *= -1.0;
      } else {
        error = -traits<T>::Local(x, origin_);
      }
    } else
#endif
    {
      if (H) {
        *H = Matrix::Identity(traits<T>::GetDimension(x),
                              traits<T>::GetDimension(x));
      }
      error = -traits<T>::Local(x, origin_);
    }
    if (mean_) {
      return error - *mean_;
    }
    return error;
  }

  using Base::error;

  double error(const T& x) const {
    return this->noiseModel_->loss(evaluateError(x));
  }

  double likelihood(const T& x) const { return exp(-error(x)); }

 
  const VALUE& origin() const { return origin_; }
  const std::optional<Vector>& mean() const { return mean_; }

  noiseModel::Gaussian::shared_ptr gaussianModel(
      const std::string& method = "<unknown>",
      bool throwOnFailure = false) const {
    using noiseModel::Gaussian;
    const auto& model = this->noiseModel();
    if (!model) {
      if (throwOnFailure) {
        throw std::runtime_error(method + " requires a noise model");
      }
      return nullptr;
    }
    auto g = std::dynamic_pointer_cast<Gaussian>(model);
    if (!g) {
      if (throwOnFailure) {
        throw std::runtime_error(method +
                                 " is only implemented for Gaussian noise "
                                 "models. The noise model used is of type " +
                                 std::string(typeid(*model).name()));
      }
      return nullptr;
    }
    return g;
  }

  std::optional<Matrix> covariance(const std::string& method = "<unknown>",
                                   bool throwOnFailure = false) const {
    auto gaussian = gaussianModel(method, throwOnFailure);
    if (!gaussian) return std::nullopt;
    return gaussian->covariance();
  }

  using Gaussian = std::pair<Vector, Matrix>;  // mean, covariance

  std::optional<Gaussian> gaussian(const std::string& method = "<unknown>",
                                   bool throwOnFailure = false) const {
    auto cov = covariance(method, throwOnFailure);
    if (!cov) return std::nullopt;
    return Gaussian{this->mean_.value_or(Vector::Zero(this->dim())), *cov};
  }

 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(origin_);
    ar& BOOST_SERIALIZATION_NVP(mean_);
  }
#endif
};

template <class VALUE>
struct traits<ExtendedPriorFactor<VALUE> >
    : public Testable<ExtendedPriorFactor<VALUE> > {};
 
}  // namespace gtsam