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
SOn.h
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
gtsam::ShonanFactor::G_
std::shared_ptr< Matrix > G_
matrix of vectorized generators
Definition: ShonanFactor.h:39
gtsam::ShonanFactor::M_
Matrix M_
measured rotation between R1 and R2
Definition: ShonanFactor.h:37
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
Rot2.h
2D rotation
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::ShonanFactor::pp_
size_t pp_
dimensionality constants
Definition: ShonanFactor.h:38
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
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::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::ShonanFactor
Definition: ShonanFactor.h:36
gtsam::ShonanFactor::Rot
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Definition: ShonanFactor.h:42
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
M1
MatrixXf M1
Definition: Tutorial_SlicingCol.cpp:1
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::equals
Definition: Testable.h:112
NonlinearFactor.h
Non-linear factor base classes.
Q2
static double Q2[8]
Definition: ndtri.c:122
gtsam
traits
Definition: SFMdata.h:40
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
Q1
static double Q1[8]
Definition: ndtri.c:94
gtsam::SO
Definition: SOn.h:54
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
j1
double j1(double x)
Definition: j1.c:174
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:12