gtsam  4.1.0
gtsam
KarcherMeanFactor-inl.h
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 
22 #include <gtsam/slam/KarcherMeanFactor.h>
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 template <class T, class ALLOC>
29 T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
30  // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
31  // No closed form solution.
32  NonlinearFactorGraph graph;
33  static const Key kKey(0);
34  for (const auto& R : rotations) {
35  graph.addPrior<T>(kKey, R);
36  }
37  Values initial;
38  initial.insert<T>(kKey, T());
39  auto result = GaussNewtonOptimizer(graph, initial).optimize();
40  return result.at<T>(kKey);
41 }
42 
43 template <class T,
44  typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
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>
61 KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
62  boost::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  }
76  whitenedJacobian_ =
77  boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
78 }
79 } // namespace gtsam
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
GaussNewtonOptimizer.h
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::KarcherMeanFactor
The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher me...
Definition: KarcherMeanFactor.h:45
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43