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