gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
ShonanFactor.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#include <type_traits>
27
28namespace gtsam {
29
35template <size_t d>
36class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
37 Matrix M_;
38 size_t p_, pp_;
39 boost::shared_ptr<Matrix> G_;
40
41 // Select Rot2 or Rot3 interface based template parameter d
42 using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
43
44public:
47
51 ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
52 const SharedNoiseModel &model = nullptr,
53 const boost::shared_ptr<Matrix> &G = nullptr);
54
58
60 void
61 print(const std::string &s,
62 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
63
65 bool equals(const NonlinearFactor &expected,
66 double tol = 1e-9) const override;
67
71
74 Vector
75 evaluateError(const SOn &Q1, const SOn &Q2,
76 boost::optional<Matrix &> H1 = boost::none,
77 boost::optional<Matrix &> H2 = boost::none) const override;
79
80private:
82 void fillJacobians(const Matrix &M1, const Matrix &M2,
83 boost::optional<Matrix &> H1,
84 boost::optional<Matrix &> H2) const;
85};
86
87// Explicit instantiation for d=2 and d=3 in .cpp file:
88using ShonanFactor2 = ShonanFactor<2>;
89using ShonanFactor3 = ShonanFactor<3>;
90
91} // namespace gtsam
2D rotation
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
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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
Template to create a binary predicate.
Definition Testable.h:111
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
Nonlinear factor base class.
Definition NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
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