Loading...
Searching...
No Matches
ShonanFactor.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>
#include <type_traits>
namespace gtsam {
template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
Matrix M_;
size_t p_, pp_;
boost::shared_ptr<Matrix> G_;
// Select Rot2 or Rot3 interface based template parameter d
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
public:
ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model = nullptr,
const boost::shared_ptr<Matrix> &G = nullptr);
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override;
Vector
evaluateError(const SOn &Q1, const SOn &Q2,
boost::optional<Matrix &> H1 = boost::none,
boost::optional<Matrix &> H2 = boost::none) const override;
private:
void fillJacobians(const Matrix &M1, const Matrix &M2,
boost::optional<Matrix &> H1,
boost::optional<Matrix &> H2) const;
};
// Explicit instantiation for d=2 and d=3 in .cpp file:
using ShonanFactor2 = ShonanFactor<2>;
using ShonanFactor3 = ShonanFactor<3>;
} // 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::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::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::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< SOn, SOn >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::ShonanFactor
ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p)...
Definition ShonanFactor.h:36
gtsam::ShonanFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition ShonanFactor.cpp:68
gtsam::ShonanFactor::evaluateError
Vector evaluateError(const SOn &Q1, const SOn &Q2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] projects down from SO(p) to the ...
Definition ShonanFactor.cpp:108
gtsam::ShonanFactor::ShonanFactor
ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, const SharedNoiseModel &model=nullptr, const boost::shared_ptr< Matrix > &G=nullptr)
Constructor.
Definition ShonanFactor.cpp:35
gtsam::ShonanFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition ShonanFactor.cpp:58