gtsam  4.1.0
gtsam
KarcherMeanFactor.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.h
14  * @author Frank Dellaert
15  * @date March 2019
16  */
17 
18 #pragma once
19 
20 #include <gtsam/base/Matrix.h>
22 
23 #include <map>
24 #include <vector>
25 
26 namespace gtsam {
32 template <class T>
33 T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
34 
35 template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
36 
45 template <class T> class KarcherMeanFactor : public NonlinearFactor {
46  // Compile time dimension: can be -1
47  enum { D = traits<T>::dimension };
48 
49  // Runtime dimension: always >=0
50  size_t d_;
51 
53  boost::shared_ptr<JacobianFactor> whitenedJacobian_;
54 
55 public:
61  template <typename CONTAINER>
62  KarcherMeanFactor(const CONTAINER &keys, int d = D,
63  boost::optional<double> beta = boost::none);
64 
66  virtual ~KarcherMeanFactor() {}
67 
69  double error(const Values &c) const override { return 0; }
70 
72  size_t dim() const override { return d_; }
73 
75  boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
76  return whitenedJacobian_;
77  }
78 };
79 // \KarcherMeanFactor
80 } // namespace gtsam
gtsam::KarcherMeanFactor::~KarcherMeanFactor
virtual ~KarcherMeanFactor()
Destructor.
Definition: KarcherMeanFactor.h:66
JacobianFactor.h
gtsam::KarcherMeanFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition: KarcherMeanFactor.h:69
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::KarcherMeanFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition: KarcherMeanFactor.h:75
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
gtsam::KarcherMeanFactor::KarcherMeanFactor
KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
Construct from given keys.
Definition: KarcherMeanFactor-inl.h:61
gtsam::KarcherMeanFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: KarcherMeanFactor.h:72
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71