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>
 
namespace gtsam {
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
 
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
 
template <class T> class KarcherMeanFactor : public NonlinearFactor {
  // Compile time dimension: can be -1
  enum { D = traits<T>::dimension };
 
  // Runtime dimension: always >=0
  size_t d_;
 
  boost::shared_ptr<JacobianFactor> whitenedJacobian_;
 
public:
  template <typename CONTAINER>
  KarcherMeanFactor(const CONTAINER &keys, int d = D,
                    boost::optional<double> beta = boost::none);
 
  ~KarcherMeanFactor() override {}
 
  double error(const Values &c) const override { return 0; }
 
  size_t dim() const override { return d_; }
 
  boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
    return whitenedJacobian_;
  }
};
// \KarcherMeanFactor
} // namespace gtsam