Loading...
Searching...
No Matches
KarcherMeanFactor-inl.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file KarcherMeanFactor.cpp
* @author Frank Dellaert
* @date March 2019
*/
#pragma once
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/KarcherMeanFactor.h>
using namespace std;
namespace gtsam {
template <class T, class ALLOC>
T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
// Cost function C(R) = \sum PriorFactor(R_i)::error(R)
// No closed form solution.
NonlinearFactorGraph graph;
static const Key kKey(0);
for (const auto& R : rotations) {
graph.addPrior<T>(kKey, R);
}
Values initial;
initial.insert<T>(kKey, T());
auto result = GaussNewtonOptimizer(graph, initial).optimize();
return result.at<T>(kKey);
}
template <class T>
T FindKarcherMean(const std::vector<T>& rotations) {
return FindKarcherMeanImpl(rotations);
}
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
return FindKarcherMeanImpl(rotations);
}
template <class T>
T FindKarcherMean(std::initializer_list<T>&& rotations) {
return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
}
template <class T>
template <typename CONTAINER>
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
boost::optional<double> beta)
: NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
if (d <= 0) {
throw std::invalid_argument(
"KarcherMeanFactor needs dimension for dynamic types.");
}
// Create the constant Jacobian made of d*d identity matrices,
// where d is the dimensionality of the manifold.
Matrix A = Matrix::Identity(d, d);
if (beta) A *= std::sqrt(*beta);
std::map<Key, Matrix> terms;
for (Key j : keys) {
terms[j] = A;
}
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
}
} // namespace gtsam
GaussNewtonOptimizer.h
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::GaussNewtonOptimizer
This class performs Gauss-Newton nonlinear optimization.
Definition GaussNewtonOptimizer.h:38
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
Definition NonlinearOptimizer.h:98
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::KarcherMeanFactor::KarcherMeanFactor
KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
Construct from given keys.
Definition KarcherMeanFactor-inl.h:60