44 bool defaultToUnit =
true);
52 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
53 using MatrixNN =
typename Rot::MatrixNN;
54 Eigen::Matrix<double, Dim, 1> vecM_;
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
68 boost::optional<Matrix&> H = boost::none)
const override {
69 return R.vec(H) - vecM_;
79 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
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);
106 Eigen::Matrix<double, Rot::dimension, Rot::dimension>
108 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
122 R2hat_H_R1_(R12.inverse().AdjointMap()) {}
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";
136 this->noiseModel_->print(
" noise model: ");
141 double tol = 1e-9)
const override {
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_;
N*N matrix representation of SO(N).
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