KarcherMeanFactor.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.h
 * @author Frank Dellaert
 * @date March 2019
 */
 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/JacobianFactor.h>
 
#include <map>
#include <vector>
#include <optional>
 
namespace gtsam {
template <class T>
typename std::enable_if<traits<T>::IsLieGroup, T>::type 
FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &elements);

template <class T>
typename std::enable_if<traits<T>::IsLieGroup, T>::type 
FindKarcherMean(std::initializer_list<T> &&elements);

template <class T> class KarcherMeanFactor : public NonlinearFactor {
  // Compile time dimension: can be -1
  inline constexpr static auto D = traits<T>::dimension;
 
  // Runtime dimension: always >=0
  size_t d_;

  std::shared_ptr<JacobianFactor> whitenedJacobian_;
 
public:
  template <typename CONTAINER>
  KarcherMeanFactor(const CONTAINER &keys, int d = D,
                    std::optional<double> beta = {});

  ~KarcherMeanFactor() override {}

  double error(const Values &c) const override { return 0; }

  size_t dim() const override { return d_; }

  std::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
    return whitenedJacobian_;
  }
};
// \KarcherMeanFactor
} // namespace gtsam