gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
26namespace gtsam {
27
42GTSAM_EXPORT SharedNoiseModel
43ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
44 bool defaultToUnit = true);
45
50template <class Rot>
51class FrobeniusPrior : public NoiseModelFactorN<Rot> {
52 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
53 using MatrixNN = typename Rot::MatrixNN;
54 Eigen::Matrix<double, Dim, 1> vecM_;
55
56 public:
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58
60 FrobeniusPrior(Key j, const MatrixNN& M,
61 const SharedNoiseModel& model = nullptr)
62 : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
63 vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
64 }
65
67 Vector evaluateError(const Rot& R,
68 boost::optional<Matrix&> H = boost::none) const override {
69 return R.vec(H) - vecM_; // Jacobian is computed only when needed.
70 }
71};
72
77template <class Rot>
78class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
79 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
80
81 public:
83 FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
84 : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
85 j2) {}
86
88 Vector evaluateError(const Rot& R1, const Rot& R2,
89 boost::optional<Matrix&> H1 = boost::none,
90 boost::optional<Matrix&> H2 = boost::none) const override {
91 Vector error = R2.vec(H2) - R1.vec(H1);
92 if (H1) *H1 = -*H1;
93 return error;
94 }
95};
96
103template <class Rot>
105 Rot R12_;
106 Eigen::Matrix<double, Rot::dimension, Rot::dimension>
107 R2hat_H_R1_;
108 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
109
110 public:
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112
115
117 FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
118 const SharedNoiseModel& model = nullptr)
119 : NoiseModelFactorN<Rot, Rot>(
120 ConvertNoiseModel(model, Dim), j1, j2),
121 R12_(R12),
122 R2hat_H_R1_(R12.inverse().AdjointMap()) {}
123
127
129 void
130 print(const std::string &s,
131 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
132 std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
133 << ">(" << keyFormatter(this->key1()) << ","
134 << keyFormatter(this->key2()) << ")\n";
135 traits<Rot>::Print(R12_, " R12: ");
136 this->noiseModel_->print(" noise model: ");
137 }
138
140 bool equals(const NonlinearFactor &expected,
141 double tol = 1e-9) const override {
142 auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
143 return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
144 traits<Rot>::Equals(this->R12_, e->R12_, tol);
145 }
146
150
152 Vector evaluateError(const Rot& R1, const Rot& R2,
153 boost::optional<Matrix&> H1 = boost::none,
154 boost::optional<Matrix&> H2 = boost::none) const override {
155 const Rot R2hat = R1.compose(R12_);
156 Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
157 Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
158 if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
159 return error;
160 }
162};
163
164} // namespace gtsam
N*N matrix representation of SO(N).
2D rotation
3D rotation represented as a rotation matrix or quaternion
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
SharedNoiseModel 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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
std::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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
Nonlinear factor base class.
Definition NonlinearFactor.h:42
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
Definition FrobeniusFactor.h:51
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:60
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:67
FrobeniusFactor calculates the Frobenius norm between rotation matrices.
Definition FrobeniusFactor.h:78
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:88
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:83
FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
Definition FrobeniusFactor.h:104
FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
Construct from two keys and measured rotation.
Definition FrobeniusFactor.h:117
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition FrobeniusFactor.h:130
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:152
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition FrobeniusFactor.h:140