gtsam  4.1.0
gtsam
FrobeniusFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/geometry/Rot2.h>
22 #include <gtsam/geometry/Rot3.h>
23 #include <gtsam/geometry/SOn.h>
25 
26 namespace gtsam {
27 
37 GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic>
38 ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
39  bool defaultToUnit = true);
40 
45 template <class Rot>
46 class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
47  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
48  using MatrixNN = typename Rot::MatrixNN;
49  Eigen::Matrix<double, Dim, 1> vecM_;
50 
51  public:
53  FrobeniusPrior(Key j, const MatrixNN& M,
54  const SharedNoiseModel& model = nullptr)
55  : NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) {
56  vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
57  }
58 
60  Vector evaluateError(const Rot& R,
61  boost::optional<Matrix&> H = boost::none) const override {
62  return R.vec(H) - vecM_; // Jacobian is computed only when needed.
63  }
64 };
65 
70 template <class Rot>
71 class GTSAM_EXPORT FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
72  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
73 
74  public:
76  FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
77  : NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
78  j2) {}
79 
81  Vector evaluateError(const Rot& R1, const Rot& R2,
82  boost::optional<Matrix&> H1 = boost::none,
83  boost::optional<Matrix&> H2 = boost::none) const override {
84  Vector error = R2.vec(H2) - R1.vec(H1);
85  if (H1) *H1 = -*H1;
86  return error;
87  }
88 };
89 
96 template <class Rot>
97 class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
98  Rot R12_;
99  Eigen::Matrix<double, Rot::dimension, Rot::dimension>
100  R2hat_H_R1_;
101  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
102 
103  public:
106 
108  FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
109  const SharedNoiseModel& model = nullptr)
110  : NoiseModelFactor2<Rot, Rot>(
111  ConvertNoiseModel(model, Dim), j1, j2),
112  R12_(R12),
113  R2hat_H_R1_(R12.inverse().AdjointMap()) {}
114 
118 
120  void
121  print(const std::string &s,
122  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
123  std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
124  << ">(" << keyFormatter(this->key1()) << ","
125  << keyFormatter(this->key2()) << ")\n";
126  traits<Rot>::Print(R12_, " R12: ");
127  this->noiseModel_->print(" noise model: ");
128  }
129 
131  bool equals(const NonlinearFactor &expected,
132  double tol = 1e-9) const override {
133  auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
134  return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) &&
135  traits<Rot>::Equals(this->R12_, e->R12_, tol);
136  }
137 
141 
143  Vector evaluateError(const Rot& R1, const Rot& R2,
144  boost::optional<Matrix&> H1 = boost::none,
145  boost::optional<Matrix&> H2 = boost::none) const override {
146  const Rot R2hat = R1.compose(R12_);
147  Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
148  Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
149  if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
150  return error;
151  }
153 };
154 
155 } // namespace gtsam
gtsam::FrobeniusBetweenFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is Frobenius norm between R1*R12 and R2.
Definition: FrobeniusFactor.h:143
SOn.h
N*N matrix representation of SO(N).
gtsam::FrobeniusFactor
FrobeniusFactor calculates the Frobenius norm between rotation matrices.
Definition: FrobeniusFactor.h:71
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::FrobeniusBetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition: FrobeniusFactor.h:131
gtsam::FrobeniusPrior::FrobeniusPrior
FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
Constructor.
Definition: FrobeniusFactor.h:53
gtsam::ConvertNoiseModel
boost::shared_ptr< noiseModel::Isotropic > ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dime...
Definition: FrobeniusFactor.cpp:27
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::FrobeniusFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is just Frobenius norm between rotation matrices.
Definition: FrobeniusFactor.h:81
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::FrobeniusPrior::evaluateError
Vector evaluateError(const Rot &R, boost::optional< Matrix & > H=boost::none) const override
Error is just Frobenius norm between Rot element and vectorized matrix M.
Definition: FrobeniusFactor.h:60
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition: types.cpp:37
Rot2.h
2D rotation
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::FrobeniusPrior
FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
Definition: FrobeniusFactor.h:46
gtsam::FrobeniusFactor::FrobeniusFactor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
Constructor.
Definition: FrobeniusFactor.h:76
gtsam::FrobeniusBetweenFactor
FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
Definition: FrobeniusFactor.h:97
gtsam::FrobeniusBetweenFactor::FrobeniusBetweenFactor
FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
Construct from two keys and measured rotation.
Definition: FrobeniusFactor.h:108
gtsam::FrobeniusBetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition: FrobeniusFactor.h:121
Rot3.h
3D rotation represented as a rotation matrix or quaternion