Loading...
Searching...
No Matches
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 FrobeniusPrior : public NoiseModelFactorN<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<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 FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<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 FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
Rot R12_;
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_;
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<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 && NoiseModelFactorN<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
Rot3.h
3D rotation represented as a rotation matrix or quaternion
SOn.h
N*N matrix representation of SO(N).
Rot2.h
2D rotation
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
gtsam::KeyFormatter
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
gtsam::ConvertNoiseModel
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
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::NoiseModelFactorN< Rot >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::FrobeniusPrior::FrobeniusPrior
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:60
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:67
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:88
gtsam::FrobeniusFactor::FrobeniusFactor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:83
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:117
gtsam::FrobeniusBetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition FrobeniusFactor.h:130
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:152
gtsam::FrobeniusBetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition FrobeniusFactor.h:140