ShonanFactor.cpp
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 #include <gtsam/sfm/ShonanFactor.h>
20 
21 #include <gtsam/base/timing.h>
24 
25 #include <cmath>
26 #include <iostream>
27 #include <vector>
28 
29 using namespace std;
30 
31 namespace gtsam {
32 
33 //******************************************************************************
34 template <size_t d>
36  const SharedNoiseModel &model,
37  const std::shared_ptr<Matrix> &G)
39  M_(R12.matrix()), // d*d in all cases
40  p_(p), // 4 for SO(4)
41  pp_(p * p), // 16 for SO(4)
42  G_(G) {
43  if (noiseModel()->dim() != d * p_)
44  throw std::invalid_argument(
45  "ShonanFactor: model with incorrect dimension.");
46  if (!G) {
47  G_ = std::make_shared<Matrix>();
48  *G_ = SOn::VectorizedGenerators(p); // expensive!
49  }
50  if (static_cast<size_t>(G_->rows()) != pp_ ||
51  static_cast<size_t>(G_->cols()) != SOn::Dimension(p))
52  throw std::invalid_argument("ShonanFactor: passed in generators "
53  "of incorrect dimension.");
54 }
55 
56 //******************************************************************************
57 template <size_t d>
58 void ShonanFactor<d>::print(const std::string &s,
59  const KeyFormatter &keyFormatter) const {
60  std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << ","
61  << keyFormatter(key<2>()) << ")\n";
62  traits<Matrix>::Print(M_, " M: ");
63  noiseModel_->print(" noise model: ");
64 }
65 
66 //******************************************************************************
67 template <size_t d>
69  double tol) const {
70  auto e = dynamic_cast<const ShonanFactor *>(&expected);
71  return e != nullptr && NoiseModelFactorN<SOn, SOn>::equals(*e, tol) &&
72  p_ == e->p_ && M_ == e->M_;
73 }
74 
75 //******************************************************************************
76 template <size_t d>
79  OptionalMatrixType H2) const {
80  gttic(ShonanFactor_Jacobians);
81  const size_t dim = p_ * d; // Stiefel manifold dimension
82 
83  if (H1) {
84  // If asked, calculate Jacobian H1 as as (M' \otimes M1) * G
85  // M' = dxd, M1 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
86  // (M' \otimes M1) is dim*dim, but last pp-dim columns are zero
87  *H1 = Matrix::Zero(dim, G_->cols());
88  for (size_t j = 0; j < d; j++) {
89  auto MG_j = M1 * G_->middleRows(j * p_, p_); // p_ * Dim(p)
90  for (size_t i = 0; i < d; i++) {
91  H1->middleRows(i * p_, p_) -= M_(j, i) * MG_j;
92  }
93  }
94  }
95  if (H2) {
96  // If asked, calculate Jacobian H2 as as (I_d \otimes M2) * G
97  // I_d = dxd, M2 = pxp, G = (p*p)xDim(p), result should be dim x Dim(p)
98  // (I_d \otimes M2) is dim*dim, but again last pp-dim columns are zero
99  H2->resize(dim, G_->cols());
100  for (size_t i = 0; i < d; i++) {
101  H2->middleRows(i * p_, p_) = M2 * G_->middleRows(i * p_, p_);
102  }
103  }
104 }
105 
106 //******************************************************************************
107 template <size_t d>
110  OptionalMatrixType H2) const {
111  gttic(ShonanFactor_evaluateError);
112 
113  const Matrix &M1 = Q1.matrix();
114  const Matrix &M2 = Q2.matrix();
115  if (M1.rows() != static_cast<int>(p_) || M2.rows() != static_cast<int>(p_))
116  throw std::invalid_argument("Invalid dimension SOn values passed to "
117  "ShonanFactor<d>::evaluateError");
118 
119  const size_t dim = p_ * d; // Stiefel manifold dimension
120  Vector fQ2(dim), hQ1(dim);
121 
122  // Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
123  fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
124 
125  // Vectorize M1*P*R12
126  const Matrix Q1PR12 = M1.leftCols<d>() * M_;
127  hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
128 
129  this->fillJacobians(M1, M2, H1, H2);
130 
131  return fQ2 - hQ1;
132 }
133 
134 /* ************************************************************************* */
135 // Explicit instantiation for d=2 and d=3
136 template class ShonanFactor<2>;
137 template class ShonanFactor<3>;
138 
139 //******************************************************************************
140 
141 } // namespace gtsam
timing.h
Timing utilities.
gtsam::ShonanFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition: ShonanFactor.cpp:58
gtsam::NoiseModelFactor::dim
size_t dim() const override
Definition: NonlinearFactor.h:240
gtsam::ShonanFactor::G_
std::shared_ptr< Matrix > G_
matrix of vectorized generators
Definition: ShonanFactor.h:39
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::SO::Dimension
static size_t Dimension(size_t n)
Definition: SOn.h:209
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
ShonanFactor.h
Main factor type in Shonan averaging, on SO(n) pairs.
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition: SOn.h:302
gtsam::ShonanFactor::pp_
size_t pp_
dimensionality constants
Definition: ShonanFactor.h:38
gtsam::ShonanFactor::fillJacobians
void fillJacobians(const Matrix &M1, const Matrix &M2, OptionalMatrixType H1, OptionalMatrixType H2) const
Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp.
Definition: ShonanFactor.cpp:77
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
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::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
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::evaluateError
Vector evaluateError(const SOn &Q1, const SOn &Q2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: ShonanFactor.cpp:108
gtsam::ShonanFactor
Definition: ShonanFactor.h:36
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
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
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::ConvertNoiseModel
SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
Definition: FrobeniusFactor.cpp:27
Q2
static double Q2[8]
Definition: ndtri.c:122
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
std
Definition: BFloat16.h:88
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::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
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::ShonanFactor::p_
size_t p_
Definition: ShonanFactor.h:38
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
FrobeniusFactor.h
Various factors that minimize some Frobenius norm.
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gttic
#define gttic(label)
Definition: timing.h:295


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15