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());
40  auto result = GaussNewtonOptimizer(graph, initial).optimize();
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
virtual const Values & optimize()
Factor Graph consisting of non-linear factors.
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
T FindKarcherMeanImpl(const vector< T, ALLOC > &rotations)
Values initial
Definition: BFloat16.h:88
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
NonlinearFactorGraph graph
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Values result
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
Eigen::Triplet< double > T
T FindKarcherMean(std::initializer_list< T > &&rotations)
traits
Definition: chartTesting.h:28
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const KeyVector keys
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::shared_ptr< JacobianFactor > whitenedJacobian_
Constant Jacobian made of d*d identity matrices.
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:29