BasisFactors.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/basis/Basis.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
 
namespace gtsam {

template <class BASIS>
class EvaluationFactor : public FunctorizedFactor<double, Vector> {
 private:
  using Base = FunctorizedFactor<double, Vector>;
 
 public:
  EvaluationFactor() {}

  EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
                   const size_t N, double x)
      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}

  EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
                   const size_t N, double x, double a, double b)
      : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
 
  virtual ~EvaluationFactor() {}
};

template <class BASIS, int M>
class VectorEvaluationFactor
    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
 private:
  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
 
 public:
  VectorEvaluationFactor() {}

  VectorEvaluationFactor(Key key, const Vector &z,
                         const SharedNoiseModel &model, const size_t N,
                         double x)
      : Base(key, z, model,
             typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}

  VectorEvaluationFactor(Key key, const Vector &z,
                         const SharedNoiseModel &model, const size_t N,
                         double x, double a, double b)
      : Base(key, z, model,
             typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
 
  virtual ~VectorEvaluationFactor() {}
};

template <class BASIS, size_t P>
class VectorComponentFactor
    : public FunctorizedFactor<double, ParameterMatrix<P>> {
 private:
  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
 
 public:
  VectorComponentFactor() {}

  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
                        const size_t N, size_t i, double x)
      : Base(key, z, model,
             typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}

  VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
                        const size_t N, size_t i, double x, double a, double b)
      : Base(
            key, z, model,
            typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
  }
 
  virtual ~VectorComponentFactor() {}
};

template <class BASIS, typename T>
class ManifoldEvaluationFactor
    : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
 private:
  using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
 
 public:
  ManifoldEvaluationFactor() {}

  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
                           const size_t N, double x)
      : Base(key, z, model,
             typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}

  ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
                           const size_t N, double x, double a, double b)
      : Base(
            key, z, model,
            typename BASIS::template ManifoldEvaluationFunctor<T>(N, x, a, b)) {
  }
 
  virtual ~ManifoldEvaluationFactor() {}
};

template <class BASIS>
class DerivativeFactor
    : public FunctorizedFactor<double, typename BASIS::Parameters> {
 private:
  using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
 
 public:
  DerivativeFactor() {}

  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
                   const size_t N, double x)
      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {}

  DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
                   const size_t N, double x, double a, double b)
      : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {}
 
  virtual ~DerivativeFactor() {}
};

template <class BASIS, int M>
class VectorDerivativeFactor
    : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
 private:
  using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
  using Func = typename BASIS::template VectorDerivativeFunctor<M>;
 
 public:
  VectorDerivativeFactor() {}

  VectorDerivativeFactor(Key key, const Vector &z,
                         const SharedNoiseModel &model, const size_t N,
                         double x)
      : Base(key, z, model, Func(N, x)) {}

  VectorDerivativeFactor(Key key, const Vector &z,
                         const SharedNoiseModel &model, const size_t N,
                         double x, double a, double b)
      : Base(key, z, model, Func(N, x, a, b)) {}
 
  virtual ~VectorDerivativeFactor() {}
};

template <class BASIS, int P>
class ComponentDerivativeFactor
    : public FunctorizedFactor<double, ParameterMatrix<P>> {
 private:
  using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
  using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
 
 public:
  ComponentDerivativeFactor() {}

  ComponentDerivativeFactor(Key key, const double &z,
                            const SharedNoiseModel &model, const size_t N,
                            size_t i, double x)
      : Base(key, z, model, Func(N, i, x)) {}

  ComponentDerivativeFactor(Key key, const double &z,
                            const SharedNoiseModel &model, const size_t N,
                            size_t i, double x, double a, double b)
      : Base(key, z, model, Func(N, i, x, a, b)) {}
 
  virtual ~ComponentDerivativeFactor() {}
};
 
}  // namespace gtsam