gtsam 4.2.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
24using namespace std;
25
26namespace gtsam {
27
28template <class T, class ALLOC>
29T 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
43template <class T>
44T FindKarcherMean(const std::vector<T>& rotations) {
45 return FindKarcherMeanImpl(rotations);
46}
47
48template <class T>
49T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
50 return FindKarcherMeanImpl(rotations);
51}
52
53template <class T>
54T FindKarcherMean(std::initializer_list<T>&& rotations) {
55 return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
56}
57
58template <class T>
59template <typename CONTAINER>
60KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
61 boost::optional<double> beta)
62 : NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
63 if (d <= 0) {
64 throw std::invalid_argument(
65 "KarcherMeanFactor needs dimension for dynamic types.");
66 }
67 // Create the constant Jacobian made of d*d identity matrices,
68 // where d is the dimensionality of the manifold.
69 Matrix A = Matrix::Identity(d, d);
70 if (beta) A *= std::sqrt(*beta);
71 std::map<Key, Matrix> terms;
72 for (Key j : keys) {
73 terms[j] = A;
74 }
75 whitenedJacobian_ =
76 boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
77}
78} // namespace gtsam
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:140
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
Construct from given keys.
Definition: KarcherMeanFactor-inl.h:60
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...