22#include <gtsam/slam/KarcherMeanFactor.h>
28template <
class T,
class ALLOC>
29T FindKarcherMeanImpl(
const vector<T, ALLOC>& rotations) {
32 NonlinearFactorGraph graph;
33 static const Key kKey(0);
34 for (
const auto& R : rotations) {
35 graph.addPrior<T>(kKey, R);
38 initial.insert<T>(kKey, T());
39 auto result = GaussNewtonOptimizer(graph, initial).optimize();
40 return result.at<T>(kKey);
44T FindKarcherMean(
const std::vector<T>& rotations) {
45 return FindKarcherMeanImpl(rotations);
49T FindKarcherMean(
const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
50 return FindKarcherMeanImpl(rotations);
54T FindKarcherMean(std::initializer_list<T>&& rotations) {
55 return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
59template <
typename CONTAINER>
61 boost::optional<double> beta)
64 throw std::invalid_argument(
65 "KarcherMeanFactor needs dimension for dynamic types.");
69 Matrix A = Matrix::Identity(d, d);
70 if (beta) A *= std::sqrt(*beta);
71 std::map<Key, Matrix> terms;
76 boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
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...