gtsam  4.1.0
gtsam
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 
28 namespace gtsam {
29 
35 template <size_t d>
36 class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<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 
44 public:
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 
80 private:
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:
90 
91 } // namespace gtsam
SOn.h
N*N matrix representation of SO(N).
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition: SOn.h:50
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
Rot2.h
2D rotation
gtsam::KeyFormatter
boost::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::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
Rot3.h
3D rotation represented as a rotation matrix or quaternion