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>
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_;

  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