FrobeniusFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2019, 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/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsam {

GTSAM_EXPORT SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
                  bool defaultToUnit = true);

template <class Rot>
class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
  using MatrixNN = typename Rot::MatrixNN;
  Eigen::Matrix<double, Dim, 1> vecM_;  

 public:
  FrobeniusPrior(Key j, const MatrixNN& M,
                 const SharedNoiseModel& model = nullptr)
      : NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
    vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
  }

  Vector evaluateError(const Rot& R,
                       boost::optional<Matrix&> H = boost::none) const override {
    return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
  }
};

template <class Rot>
class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
  enum { Dim = Rot::VectorN2::RowsAtCompileTime };

 public:
  FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
      : NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
                                    j2) {}

  Vector evaluateError(const Rot& R1, const Rot& R2,
                       boost::optional<Matrix&> H1 = boost::none,
                       boost::optional<Matrix&> H2 = boost::none) const override {
    Vector error = R2.vec(H2) - R1.vec(H1);
    if (H1) *H1 = -*H1;
    return error;
  }
};

template <class Rot>
class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
  Rot R12_;  
  Eigen::Matrix<double, Rot::dimension, Rot::dimension>
      R2hat_H_R1_;  
  enum { Dim = Rot::VectorN2::RowsAtCompileTime };

 public:

  FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
                         const SharedNoiseModel& model = nullptr)
      : NoiseModelFactor2<Rot, Rot>(
            ConvertNoiseModel(model, Dim), j1, j2),
        R12_(R12),
        R2hat_H_R1_(R12.inverse().AdjointMap()) {}


  void
  print(const std::string &s,
        const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
              << ">(" << keyFormatter(this->key1()) << ","
              << keyFormatter(this->key2()) << ")\n";
    traits<Rot>::Print(R12_, "  R12: ");
    this->noiseModel_->print("  noise model: ");
  }

  bool equals(const NonlinearFactor &expected,
              double tol = 1e-9) const override {
    auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
    return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
           traits<Rot>::Equals(this->R12_, e->R12_, tol);
  }


  Vector evaluateError(const Rot& R1, const Rot& R2,
                       boost::optional<Matrix&> H1 = boost::none,
                       boost::optional<Matrix&> H2 = boost::none) const override {
    const Rot R2hat = R1.compose(R12_);
    Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
    Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
    if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
    return error;
  }
};

}  // namespace gtsam