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 NoiseModelFactorN<SOn, SOn> {
38  size_t p_, pp_;
39  std::shared_ptr<Matrix> G_;
40 
41  // Select Rot2 or Rot3 interface based template parameter d
43 
44 public:
45 
46  // Provide access to the Matrix& version of evaluateError:
47  using NoiseModelFactor2<SOn, SOn>::evaluateError;
48 
51 
55  ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
56  const SharedNoiseModel &model = nullptr,
57  const std::shared_ptr<Matrix> &G = nullptr);
58 
62 
64  void
65  print(const std::string &s,
66  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
67 
69  bool equals(const NonlinearFactor &expected,
70  double tol = 1e-9) const override;
71 
75 
78  Vector evaluateError(const SOn& Q1, const SOn& Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override;
80 
81 private:
83  void fillJacobians(const Matrix &M1, const Matrix &M2,
85  OptionalMatrixType H2) const;
86 };
87 
88 // Explicit instantiation for d=2 and d=3 in .cpp file:
91 
92 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
std::shared_ptr< Matrix > G_
matrix of vectorized generators
Definition: ShonanFactor.h:39
JacobiRotation< float > G
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:971
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
MatrixXf M1
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Definition: SOn.h:54
size_t pp_
dimensionality constants
Definition: ShonanFactor.h:38
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
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
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
float * p
const G double tol
Definition: Group.h:86
Matrix M_
measured rotation between R1 and R2
Definition: ShonanFactor.h:37
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
2D rotation
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Definition: ShonanFactor.h:42
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:43