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>
class VectorEvaluationFactor : public FunctorizedFactor<Vector, Matrix> {
 private:
  using Base = FunctorizedFactor<Vector, Matrix>;
 
 public:
  VectorEvaluationFactor() {}

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

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

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

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

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

template <class BASIS, typename T>
class ManifoldEvaluationFactor : public FunctorizedFactor<T, Matrix> {
 private:
  using Base = FunctorizedFactor<T, Matrix>;
 
 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>
class VectorDerivativeFactor : public FunctorizedFactor<Vector, Matrix> {
 private:
  using Base = FunctorizedFactor<Vector, Matrix>;
  using Func = typename BASIS::VectorDerivativeFunctor;
 
 public:
  VectorDerivativeFactor() {}

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

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

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

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

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