KarcherMeanFactor-inl.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, 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 
12 /*
13  * @file KarcherMeanFactor.cpp
14  * @author Frank Dellaert
15  * @date March 2019
16  */
17 
18 #pragma once
19 
23 #include <optional>
24 
25 using namespace std;
26 
27 namespace gtsam {
28 
29 template <class T, class ALLOC>
30 T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
31  // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
32  // No closed form solution.
34  static const Key kKey(0);
35  for (const auto& R : rotations) {
36  graph.addPrior<T>(kKey, R);
37  }
39  initial.insert<T>(kKey, T());
41  return result.at<T>(kKey);
42 }
43 
44 template <class T>
45 T FindKarcherMean(const std::vector<T>& rotations) {
46  return FindKarcherMeanImpl(rotations);
47 }
48 
49 template <class T>
50 T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
51  return FindKarcherMeanImpl(rotations);
52 }
53 
54 template <class T>
55 T FindKarcherMean(std::initializer_list<T>&& rotations) {
56  return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
57 }
58 
59 template <class T>
60 template <typename CONTAINER>
62  std::optional<double> beta)
63  : NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
64  if (d <= 0) {
65  throw std::invalid_argument(
66  "KarcherMeanFactor needs dimension for dynamic types.");
67  }
68  // Create the constant Jacobian made of d*d identity matrices,
69  // where d is the dimensionality of the manifold.
70  Matrix A = Matrix::Identity(d, d);
71  if (beta) A *= std::sqrt(*beta);
72  std::map<Key, Matrix> terms;
73  for (Key j : keys) {
74  terms[j] = A;
75  }
77  std::make_shared<JacobianFactor>(terms, Vector::Zero(d));
78 }
79 } // namespace gtsam
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
d
static const double d[K][N]
Definition: igam.h:11
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::KarcherMeanFactor::whitenedJacobian_
std::shared_ptr< JacobianFactor > whitenedJacobian_
Constant Jacobian made of d*d identity matrices.
Definition: KarcherMeanFactor.h:54
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
result
Values result
Definition: OdometryOptimize.cpp:8
beta
double beta(double a, double b)
Definition: beta.c:61
gtsam::KarcherMeanFactor
Definition: KarcherMeanFactor.h:46
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
GaussNewtonOptimizer.h
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
gtsam::FindKarcherMean
T FindKarcherMean(std::initializer_list< T > &&rotations)
Definition: KarcherMeanFactor-inl.h:55
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
Eigen::Triplet< double >
KarcherMeanFactor.h
size_t
std::size_t size_t
Definition: wrap/pybind11/include/pybind11/detail/common.h:490
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::FindKarcherMeanImpl
T FindKarcherMeanImpl(const vector< T, ALLOC > &rotations)
Definition: KarcherMeanFactor-inl.h:30
initial
Definition: testScenarioRunner.cpp:148
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
R
Rot2 R(Rot2::fromAngle(0.1))


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