From 2687c24cc2fd9f8adf07fb325554713e45d6195d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 5 Jul 2020 12:24:38 -0400 Subject: [PATCH 01/36] added Basis classes and corresponding tests --- gtsam/CMakeLists.txt | 1 + gtsam/basis/Basis.cpp | 43 +++ gtsam/basis/Basis.h | 358 ++++++++++++++++++++++++ gtsam/basis/BasisFactors.h | 82 ++++++ gtsam/basis/CMakeLists.txt | 6 + gtsam/basis/Chebyshev.h | 103 +++++++ gtsam/basis/Chebyshev2.cpp | 197 +++++++++++++ gtsam/basis/Chebyshev2.h | 141 ++++++++++ gtsam/basis/FitBasis.h | 85 ++++++ gtsam/basis/Fourier.h | 71 +++++ gtsam/basis/tests/CMakeLists.txt | 1 + gtsam/basis/tests/testBasis.cpp | 9 + gtsam/basis/tests/testChebyshev.cpp | 179 ++++++++++++ gtsam/basis/tests/testChebyshev2.cpp | 398 +++++++++++++++++++++++++++ gtsam/basis/tests/testFourier.cpp | 250 +++++++++++++++++ 15 files changed, 1924 insertions(+) create mode 100644 gtsam/basis/Basis.cpp create mode 100644 gtsam/basis/Basis.h create mode 100644 gtsam/basis/BasisFactors.h create mode 100644 gtsam/basis/CMakeLists.txt create mode 100644 gtsam/basis/Chebyshev.h create mode 100644 gtsam/basis/Chebyshev2.cpp create mode 100644 gtsam/basis/Chebyshev2.h create mode 100644 gtsam/basis/FitBasis.h create mode 100644 gtsam/basis/Fourier.h create mode 100644 gtsam/basis/tests/CMakeLists.txt create mode 100644 gtsam/basis/tests/testBasis.cpp create mode 100644 gtsam/basis/tests/testChebyshev.cpp create mode 100644 gtsam/basis/tests/testChebyshev2.cpp create mode 100644 gtsam/basis/tests/testFourier.cpp diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 16dca6736f..c26fe13236 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic diff --git a/gtsam/basis/Basis.cpp b/gtsam/basis/Basis.cpp new file mode 100644 index 0000000000..7edf862613 --- /dev/null +++ b/gtsam/basis/Basis.cpp @@ -0,0 +1,43 @@ +/** + * @file Basis.cpp + * @brief Compute an interpolating basis + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#include + +#include + +namespace gtsam { + +template +Matrix Basis::WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; +} + +template +Matrix Basis::WeightMatrix(size_t N, const Vector& X, double a, + double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; +} + +// Define all the functions with `cout` outside the header + +template +void Basis::EvaluationFunctor::print(const std::string& s) const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; +} + +template +void Basis::DerivativeFunctorBase::print(const std::string& s) const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; +} + +} // namespace gtsam diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 0000000000..3b98540cc7 --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,358 @@ +/** + * @file Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +/* + * Concept of a Basis: + * - type Weights, Parameters + * - CalculateWeights(size_t N, double a=default, double b=default) + */ + +namespace gtsam { + +/// CRTP Base class for function bases +template +class Basis { + public: + /// Call weights for all x in vector, returns M*N matrix where M is the size + /// of the vector X. + static Matrix WeightMatrix(size_t N, const Vector& X); + + /// Call weights for all x in vector, with interval [a,b] + /// Returns M*N matrix where M is the size of the vector X. + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b); + + /** + * EvaluationFunctor at a given x, applied to Parameters. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobian wrpt the parameters. + */ + class EvaluationFunctor { + protected: + typename DERIVED::Weights weights_; + + public: + // Used by FunctorizedFactor + using argument_type = Vector; + using return_type = double; + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& f, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * f)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& f, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(f, H); // might call apply in derived + } + + void print(const std::string& s = "") const; + }; + + /** + * VectorEvaluationFunctor at a given x, applied to MatrixMN. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobian wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + typedef Eigen::Matrix VectorM; + typedef Eigen::Matrix MatrixMN; + typedef Eigen::Matrix Jacobian; + Jacobian H_; + void calculateJacobian() { + typedef Eigen::Matrix MatrixM; + H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // Used by FunctorizedFactor + using argument_type = Matrix; + using return_type = Vector; + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return F * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + return apply(F, H); + } + }; + + /** + * Given M*N Matrix of M-vectors at N Chebyshev points, predict component for + * given row i, with 0 + class ComponentEvaluationFunctor : public EvaluationFunctor { + protected: + typedef Eigen::Matrix MatrixMN; + typedef Eigen::Matrix Jacobian; + size_t rowIndex_; + Jacobian H_; + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + // Used by FunctorizedFactor + using argument_type = Matrix; + using return_type = double; + + /// Construct with row index + ComponentEvaluationFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentEvaluationFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of F + double apply(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return F.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + return apply(F, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to MatrixMN. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobian wrpt the parameters. + */ + /// Manifold interpolation + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + // Used by FunctorizedFactor + using argument_type = Matrix; + using return_type = T; + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const typename Base::MatrixMN& F, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(F, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const typename Base::MatrixMN& F, + OptionalJacobian H = boost::none) const { + return apply(F, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculates weights + class DerivativeFunctorBase { + protected: + typename DERIVED::Weights weights_; + + public: + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const; + }; + + /// Given values f at the Chebyshev points, predict derivative at x + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + // Used by FunctorizedFactor + using argument_type = typename DERIVED::Parameters; + using return_type = double; + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& f, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * f)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& f, + OptionalJacobian H = boost::none) const { + return apply(f, H); // might call apply in derived + } + }; + + /// Vector interpolation, e.g. given 3*N matrix yields 3-vector + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + typedef Eigen::Matrix VectorM; + typedef Eigen::Matrix MatrixMN; + typedef Eigen::Matrix Jacobian; + Jacobian H_; + void calculateJacobian() { + typedef Eigen::Matrix MatrixM; + H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); + } + + public: + // Used by FunctorizedFactor + using argument_type = Matrix; + using return_type = Vector; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return F * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()(const MatrixMN& F, OptionalJacobian H = + boost::none) const { + return apply(F, H); + } + }; + + /** + * Given M*N Matrix of M-vectors at N Chebyshev points, predict derivative for + * given row i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + typedef Eigen::Matrix MatrixMN; + typedef Eigen::Matrix Jacobian; + size_t rowIndex_; + Jacobian H_; + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + // Used by FunctorizedFactor + using argument_type = Matrix; + using return_type = double; + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return F.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const MatrixMN& F, + OptionalJacobian H = boost::none) const { + return apply(F, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& f, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(f.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 0000000000..72df8c8e99 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 BasisFactors.h + * @author Varun Agrawal + **/ + +#pragma once + +#include "FunctorizedFactor.h" + +namespace gtsam { + +/** + * Factor for BASIS evaluation. + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +using PredictFactor = FunctorizedFactor; + +/** + * Factor for BASIS derivative evaluation. + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +using DerivativeFactor = FunctorizedFactor; + +/** + * Prior factor for BASIS vector evaluation on Matrix of size (M, N_ORDER). + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +using VectorPrior = + FunctorizedFactor>; + +/** + * Prior factor for BASIS Manifold evaluation on type T. + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the functor. + */ +template +using TypePrior = + FunctorizedFactor>; + +/** + * Prior factor for BASIS component evaluation on Matrix of size (M, N_ORDER). + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the component. + */ +template +using ComponentPrior = + FunctorizedFactor>; + +/** + * Prior factor for BASIS vector derivative on Matrix of size (M, N_ORDER). + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +using VectorDerivativePrior = + FunctorizedFactor>; + +/** + * Prior factor for BASIS component derivative on Matrix of size (M, N_ORDER). + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the component derivative. + */ +template +using ComponentDerivativePrior = + FunctorizedFactor>; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 0000000000..203fd96a2d --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 0000000000..23ea763087 --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -0,0 +1,103 @@ +/** + * @file Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +class Chebyshev2Basis : public Basis { + public: + using Weights = Eigen::Matrix; + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + */ + static Weights CalculateWeights(size_t N, double x) { + Weights Ux(N); + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; + } +}; +// Chebyshev2Basis + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +class Chebyshev1Basis : public Basis { + public: + using Weights = Eigen::Matrix; + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + */ + static Weights CalculateWeights(size_t N, double x) { + Weights Tx(1, N); + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; + } + + /// Create a Chebyshev derivative function of Parameters + class Derivative { + protected: + size_t N_; + + public: + // From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + // I.e. the derivative fo a first kind cheb is just a second kind cheb + // So, we define a second kind basis here of order N-1 + // Note that it has one less weight: + typename Chebyshev2Basis::Weights weights_; + + Derivative(size_t N, double x) + : weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)), N_(N) { + // after the above init, weights_ contains the U_{n-1} values for n=1..N-1 + // Note there is no value for n=0. But we need n*U{n-1}, so correct: + for (size_t n = 1; n < N; n++) { + weights_(n - 1) *= double(n); + } + } + + double operator()(const Parameters &c) { + // The Parameters pertain to 1st kind chebyshevs up to order N-1 + // But of course the first one (order 0) is constant, so omit that weight + return (weights_ * c.block(1, 0, N_ - 1, 1))(0); + } + }; +}; +// Chebyshev1Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 0000000000..9089eb91e7 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,197 @@ +/** + * @file Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Chebyshev2::Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, + double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Chebyshev2::Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, + double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Chebyshev2::Weights Chebyshev2::IntegrationWeights(size_t N, double a, + double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 0000000000..1386a49830 --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,141 @@ +/** + * @file Chebyshev2.h + * @brief Chebyshev parameterizations on Chebyshev points of second kind. + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis + * functions, rather estimate function parameters that enforce function + * nodes at Chebyshev points. + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the #points, is one less than N from Trefethen00book + * (pg.42) + */ +class Chebyshev2 : public Basis { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + public: + using Base = Basis; + using Weights = Eigen::Matrix; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * A matrix of M*N values at the Chebyshev points, where M is the dimension of + * T template argument T: the type you would like to EvaluationFunctor using + * polynomial interpolation template argument N: the number of Chebyshev + * points of the second kind + */ + template + struct ParameterMatrix { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // First dimension is: traits::dimension + typedef Eigen::Matrix type; + }; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + * + * Please refer to Trefethen13chapters p 35, formula 5.13 + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix( + boost::function(double)> f, // + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) Xmat.col(j) = f(Point(N, j, a, b)); + return Xmat; + } +}; +// \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 0000000000..32041c8e72 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include + +#include "Basis.h" +#include "BasisFactors.h" + +namespace gtsam { + +/// For now, this is our sequence representation +typedef std::map Sequence; + +/** + * Class that does Fourier Decomposition via least squares + */ +template +class FitBasis { + public: + typedef std::pair Sample; + typedef typename Basis::Parameters Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return std::move(graph); + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /// Constructor + FitBasis(size_t N, const Sequence& sequence, const SharedNoiseModel& model) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 0000000000..28453b0e8e --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,71 @@ +/** + * @file Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class FourierBasis : public Basis { + public: + using Weights = Eigen::Matrix; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * Evaluate Real Fourier Weights of size N + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i += 2, n++) { + b[i] = cos(n * x); + b[i + 1] = sin(n * x); + } + return b; + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /// Create a Fourier derivative function of Parameters + class Derivative { + protected: + typename FourierBasis::Weights weights_; + + size_t N_; + + public: + Derivative(size_t N, double x) + : weights_(FourierBasis::CalculateWeights(N - 1, x)), N_(N) {} + + /// Get weights at a given x that calculate the derivative. + Weights operator()(const Parameters &c) { + return CalculateWeights(N_, x) * DifferentiationMatrix(N_); + } + }; + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 0000000000..8e99ef5ba4 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(base "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testBasis.cpp b/gtsam/basis/tests/testBasis.cpp new file mode 100644 index 0000000000..d8ee5fa56d --- /dev/null +++ b/gtsam/basis/tests/testBasis.cpp @@ -0,0 +1,9 @@ +#include +#include + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 0000000000..8e9c757e24 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,179 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include + +#include "../Chebyshev.h" +#include "../FitBasis.h" + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + typedef Chebyshev1Basis::EvaluationFunctor Synth; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + typedef Chebyshev2Basis::EvaluationFunctor Synth; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Prediction) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(N, sequence, model); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + typedef Chebyshev1Basis::Derivative D; + Vector c(N); + double x; + c << 12, 3, 1; + // D[12 + 3*x + 2*x*x-1,x] = 3 + 4*x + x = -1.0; + EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); +} + +//****************************************************************************** +Vector3 f(1, 0, 1); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + Chebyshev1Basis::Derivative dTdx(N, x); + double analytic_dTdx = f(1) + 4 * f(2) * x; // not too hard to do + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), analytic_dTdx, 1e-9); + EXPECT_DOUBLES_EQUAL(analytic_dTdx, dTdx(f), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 0000000000..c6f147f037 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,398 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include + +#include "../Chebyshev2.h" +#include "../FitBasis.h" + +using namespace std; +using namespace gtsam; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + Matrix X = Matrix::Zero(2, 3); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = numericalDerivative11(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(3, sequence, model); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + const size_t N = 32; + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Chebyshev2::Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Chebyshev2::Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + const size_t N = 32; + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Chebyshev2::Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Chebyshev2::Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = 185.454784; + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + const size_t N = 32; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Chebyshev2::Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Chebyshev2::Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3); + Chebyshev2::Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights2) { + const size_t N = 32; + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Chebyshev2::Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Chebyshev2::Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Chebyshev2::Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Chebyshev2::Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-4)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Chebyshev2::Weights expected1 = + Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Chebyshev2::Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-4)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + typedef Chebyshev2::VectorDerivativeFunctor VecD; + VecD fx(N, x, 0, 3); + Matrix X = Matrix::Zero(M, N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 7, M = 3; + const double x = 0.2; + typedef Chebyshev2::ComponentDerivativeFunctor CompFunc; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + Matrix X = Matrix::Zero(M, 7); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + Matrix expectedH = numericalDerivative11( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 0000000000..2790e24e20 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,250 @@ +/* ---------------------------------------------------------------------------- + + * 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 testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert + * @brief unit tests for Fourier Basis Decompositions w Expressions + */ + +#include +#include +#include + +#include "../FitBasis.h" +#include "../Fourier.h" + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + PredictFactor predictFactor(key, desiredValue, model, N, x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto gaussianFactor = predictFactor.linearize(values); + auto jacobianFactor = + boost::dynamic_pointer_cast(gaussianFactor); + CHECK(jacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *jacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, PredictFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, model, + 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(3, sequence, model); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From a5e701af71c60f9b31bec0f6b391ec8dc3ad200d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 13 Jul 2020 04:31:32 -0400 Subject: [PATCH 02/36] Updated Basis code to use the new FunctorizedFactor --- gtsam/basis/Basis.cpp | 43 ------------- gtsam/basis/Basis.h | 23 +++++-- gtsam/basis/BasisFactors.h | 35 +++++++---- gtsam/basis/Chebyshev.h | 2 +- gtsam/basis/FitBasis.h | 7 ++- gtsam/basis/Fourier.h | 20 ++---- gtsam/basis/tests/testBasis.cpp | 9 --- gtsam/basis/tests/testChebyshev.cpp | 10 +-- gtsam/basis/tests/testChebyshev2.cpp | 8 +-- gtsam/basis/tests/testFourier.cpp | 92 ++++++++++++++-------------- gtsam/nonlinear/FunctorizedFactor.h | 3 +- 11 files changed, 106 insertions(+), 146 deletions(-) delete mode 100644 gtsam/basis/Basis.cpp delete mode 100644 gtsam/basis/tests/testBasis.cpp diff --git a/gtsam/basis/Basis.cpp b/gtsam/basis/Basis.cpp deleted file mode 100644 index 7edf862613..0000000000 --- a/gtsam/basis/Basis.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/** - * @file Basis.cpp - * @brief Compute an interpolating basis - * @author Varun Agrawal - * @date July 4, 2020 - */ - -#include - -#include - -namespace gtsam { - -template -Matrix Basis::WeightMatrix(size_t N, const Vector& X) { - Matrix W(X.size(), N); - for (int i = 0; i < X.size(); i++) - W.row(i) = DERIVED::CalculateWeights(N, X(i)); - return W; -} - -template -Matrix Basis::WeightMatrix(size_t N, const Vector& X, double a, - double b) { - Matrix W(X.size(), N); - for (int i = 0; i < X.size(); i++) - W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); - return W; -} - -// Define all the functions with `cout` outside the header - -template -void Basis::EvaluationFunctor::print(const std::string& s) const { - std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; -} - -template -void Basis::DerivativeFunctorBase::print(const std::string& s) const { - std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; -} - -} // namespace gtsam diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 3b98540cc7..4ce9baee66 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -11,6 +11,7 @@ #include #include +#include /* * Concept of a Basis: @@ -26,11 +27,21 @@ class Basis { public: /// Call weights for all x in vector, returns M*N matrix where M is the size /// of the vector X. - static Matrix WeightMatrix(size_t N, const Vector& X); + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } /// Call weights for all x in vector, with interval [a,b] /// Returns M*N matrix where M is the size of the vector X. - static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b); + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } /** * EvaluationFunctor at a given x, applied to Parameters. @@ -68,7 +79,9 @@ class Basis { return apply(f, H); // might call apply in derived } - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } }; /** @@ -232,7 +245,9 @@ class Basis { DerivativeFunctorBase(size_t N, double x, double a, double b) : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} - void print(const std::string& s = "") const; + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } }; /// Given values f at the Chebyshev points, predict derivative at x diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 72df8c8e99..f11ec9e53f 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -16,7 +16,7 @@ #pragma once -#include "FunctorizedFactor.h" +#include namespace gtsam { @@ -25,14 +25,18 @@ namespace gtsam { * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -using PredictFactor = FunctorizedFactor; +using PredictFactor = + FunctorizedFactor; /** * Factor for BASIS derivative evaluation. * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -using DerivativeFactor = FunctorizedFactor; +using DerivativeFactor = + FunctorizedFactor; /** * Prior factor for BASIS vector evaluation on Matrix of size (M, N_ORDER). @@ -40,8 +44,9 @@ using DerivativeFactor = FunctorizedFactor; * @param M: Size of the evaluated state vector. */ template -using VectorPrior = - FunctorizedFactor>; +using VectorPrior = FunctorizedFactor< + typename BASIS::template VectorEvaluationFunctor::return_type, + typename BASIS::template VectorEvaluationFunctor::argument_type>; /** * Prior factor for BASIS Manifold evaluation on type T. @@ -49,8 +54,9 @@ using VectorPrior = * @param T: Object type which is synthesized by the functor. */ template -using TypePrior = - FunctorizedFactor>; +using TypePrior = FunctorizedFactor< + typename BASIS::template ManifoldEvaluationFunctor::return_type, + typename BASIS::template ManifoldEvaluationFunctor::argument_type>; /** * Prior factor for BASIS component evaluation on Matrix of size (M, N_ORDER). @@ -58,8 +64,9 @@ using TypePrior = * @param P: Size of the component. */ template -using ComponentPrior = - FunctorizedFactor>; +using ComponentPrior = FunctorizedFactor< + typename BASIS::template ComponentEvaluationFunctor

::return_type, + typename BASIS::template ComponentEvaluationFunctor

::argument_type>; /** * Prior factor for BASIS vector derivative on Matrix of size (M, N_ORDER). @@ -67,8 +74,9 @@ using ComponentPrior = * @param M: Size of the evaluated state vector derivative. */ template -using VectorDerivativePrior = - FunctorizedFactor>; +using VectorDerivativePrior = FunctorizedFactor< + typename BASIS::template VectorDerivativeFunctor::return_type, + typename BASIS::template VectorDerivativeFunctor::argument_type>; /** * Prior factor for BASIS component derivative on Matrix of size (M, N_ORDER). @@ -76,7 +84,8 @@ using VectorDerivativePrior = * @param P: Size of the component derivative. */ template -using ComponentDerivativePrior = - FunctorizedFactor>; +using ComponentDerivativePrior = FunctorizedFactor< + typename BASIS::template ComponentDerivativeFunctor

::return_type, + typename BASIS::template ComponentDerivativeFunctor

::argument_type>; } // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 23ea763087..4a2af0635b 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -83,7 +83,7 @@ class Chebyshev1Basis : public Basis { typename Chebyshev2Basis::Weights weights_; Derivative(size_t N, double x) - : weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)), N_(N) { + : N_(N), weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)) { // after the above init, weights_ contains the U_{n-1} values for n=1..N-1 // Note there is no value for n=0. But we need n*U{n-1}, so correct: for (size_t n = 1; n < N; n++) { diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 32041c8e72..213d5a995e 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -55,10 +55,11 @@ class FitBasis { size_t N) { NonlinearFactorGraph graph; for (const Sample sample : sequence) { - graph.emplace_shared>(0, sample.second, model, N, - sample.first); + auto functor = typename Basis::EvaluationFunctor(N, sample.first); + graph.emplace_shared>(0, sample.second, model, + functor); } - return std::move(graph); + return graph; } /// Create linear FG from Sequence diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 28453b0e8e..c07ce275e0 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -49,22 +49,10 @@ class FourierBasis : public Basis { return D; } - /// Create a Fourier derivative function of Parameters - class Derivative { - protected: - typename FourierBasis::Weights weights_; - - size_t N_; - - public: - Derivative(size_t N, double x) - : weights_(FourierBasis::CalculateWeights(N - 1, x)), N_(N) {} - - /// Get weights at a given x that calculate the derivative. - Weights operator()(const Parameters &c) { - return CalculateWeights(N_, x) * DifferentiationMatrix(N_); - } - }; + /// Get weights at a given x that calculate the derivative. + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } }; // FourierBasis diff --git a/gtsam/basis/tests/testBasis.cpp b/gtsam/basis/tests/testBasis.cpp deleted file mode 100644 index d8ee5fa56d..0000000000 --- a/gtsam/basis/tests/testBasis.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index 8e9c757e24..bb987405fa 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -18,11 +18,10 @@ #include #include +#include +#include #include -#include "../Chebyshev.h" -#include "../FitBasis.h" - using namespace std; using namespace gtsam; @@ -83,8 +82,9 @@ TEST(Chebyshev, Expression) { x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; for (size_t i = 0; i < m; i++) { - graph.emplace_shared>(key, x(i), model, N, - t(i)); + auto functor = Chebyshev1Basis::EvaluationFunctor(N, t(i)); + graph.emplace_shared>(key, x(i), model, + functor); } // Solve diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index c6f147f037..e20c347981 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -13,16 +13,16 @@ * @file testChebyshev.cpp * @date July 4, 2020 * @author Varun Agrawal - * @brief Unit tests for Chebyshev Basis Decompositions + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods */ #include #include +#include +#include #include -#include "../Chebyshev2.h" -#include "../FitBasis.h" - using namespace std; using namespace gtsam; diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index 2790e24e20..9f853ec1bd 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -13,16 +13,15 @@ * @file testFourier.cpp * @date July 4, 2020 * @author Frank Dellaert - * @brief unit tests for Fourier Basis Decompositions w Expressions + * @brief unit tests for Fourier Basis Decompositions */ #include #include +#include +#include #include -#include "../FitBasis.h" -#include "../Fourier.h" - using namespace std; using namespace gtsam; @@ -84,7 +83,8 @@ TEST(Basis, Manual) { graph.add(linearFactor); // Create factor to predict value at x - PredictFactor predictFactor(key, desiredValue, model, N, x); + PredictFactor predictFactor( + key, desiredValue, model, FourierBasis::EvaluationFunctor(N, x)); // Check expression Jacobians EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); @@ -110,8 +110,8 @@ TEST(Basis, PredictFactor) { Key key(1); for (size_t i = 0; i < 16; i++) { double x = i * M_PI / 8, desiredValue = TestFunction(x); - graph.emplace_shared>(key, desiredValue, model, - 7, x); + graph.emplace_shared>( + key, desiredValue, model, FourierBasis::EvaluationFunctor(7, x)); } // Solve FourierFactorGraph @@ -202,45 +202,45 @@ TEST(Basis, VecDerivativeFunctor) { // we can simply express the coefficents c as c=inv(W(X))*f, which is a // generalized Fourier transform. That also means we can create factors with the // unknown f-values, as done manually below. -TEST(Basis, PseudoSpectral) { - // We will create a linear factor graph - GaussianFactorGraph graph; - - const size_t N = 3; - const Key key(1); - - // The correct values at X = {0.1,0.2,0.3} are simply W*c - const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); - const Matrix W = FourierBasis::WeightMatrix(N, X); - const Vector expected = W * k3Coefficients; - - // Check those values are indeed correct values of Fourier approximation - using Eval = FourierBasis::EvaluationFunctor; - EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); - EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); - EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); - - // Calculate "inverse Fourier transform" matrix - const Matrix invW = W.inverse(); - - // At 16 different samples points x, add a factor on fExpr - for (size_t i = 0; i < 16; i++) { - const double x = i * M_PI / 8; - const double desiredValue = TestFunction(x); - - // Manual JacobianFactor - Matrix A(1, 3); - A << 1, cos(x), sin(x); - Vector b(1); - b << desiredValue; - JacobianFactor linearFactor(key, A * invW, b); - graph.add(linearFactor); - } - - // Solve linear graph - VectorValues actual = graph.optimize(); - EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); -} +// TEST(Basis, PseudoSpectral) { +// // We will create a linear factor graph +// GaussianFactorGraph graph; + +// const size_t N = 3; +// const Key key(1); + +// // The correct values at X = {0.1,0.2,0.3} are simply W*c +// const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); +// const Matrix W = FourierBasis::WeightMatrix(N, X); +// const Vector expected = W * k3Coefficients; + +// // Check those values are indeed correct values of Fourier approximation +// using Eval = FourierBasis::EvaluationFunctor; +// EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); +// EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); +// EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + +// // Calculate "inverse Fourier transform" matrix +// const Matrix invW = W.inverse(); + +// // At 16 different samples points x, add a factor on fExpr +// for (size_t i = 0; i < 16; i++) { +// const double x = i * M_PI / 8; +// const double desiredValue = TestFunction(x); + +// // Manual JacobianFactor +// Matrix A(1, 3); +// A << 1, cos(x), sin(x); +// Vector b(1); +// b << desiredValue; +// JacobianFactor linearFactor(key, A * invW, b); +// graph.add(linearFactor); +// } + +// // Solve linear graph +// VectorValues actual = graph.optimize(); +// EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +// } //****************************************************************************** int main() { diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index a831989674..6002bb8794 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -109,8 +109,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const { const FunctorizedFactor *e = dynamic_cast *>(&other); - const bool base = Base::equals(*e, tol); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} From 54b4547d7c49d58965ba1c7327c8ffdfa5071cf9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 13 Jul 2020 04:31:52 -0400 Subject: [PATCH 03/36] Added basis to doc cmake file --- doc/CMakeLists.txt | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a89892..2218addcfc 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) From 42974a7e6e1cde4ee15e4637ff0ba7dda1dccf4f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 4 Jun 2021 13:30:31 -0400 Subject: [PATCH 04/36] update pseudospectral code to the latest version --- gtsam/basis/Basis.h | 120 ++++++------ gtsam/basis/BasisFactors.h | 202 ++++++++++++++++---- gtsam/basis/Chebyshev.cpp | 45 +++++ gtsam/basis/Chebyshev.h | 51 +++-- gtsam/basis/Chebyshev2.h | 42 ++--- gtsam/basis/FitBasis.h | 16 +- gtsam/basis/ParameterMatrix.h | 218 ++++++++++++++++++++++ gtsam/basis/tests/testChebyshev.cpp | 20 +- gtsam/basis/tests/testChebyshev2.cpp | 87 ++++++--- gtsam/basis/tests/testFourier.cpp | 99 +++++----- gtsam/basis/tests/testParameterMatrix.cpp | 145 ++++++++++++++ gtsam/gtsam.i | 131 +++++++++++++ 12 files changed, 938 insertions(+), 238 deletions(-) create mode 100644 gtsam/basis/Chebyshev.cpp create mode 100644 gtsam/basis/ParameterMatrix.h create mode 100644 gtsam/basis/tests/testParameterMatrix.cpp diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 4ce9baee66..f1747f19aa 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 Basis.h * @brief Compute an interpolating basis @@ -9,11 +20,12 @@ #include #include +#include #include #include -/* +/** * Concept of a Basis: * - type Weights, Parameters * - CalculateWeights(size_t N, double a=default, double b=default) @@ -54,9 +66,8 @@ class Basis { typename DERIVED::Weights weights_; public: - // Used by FunctorizedFactor - using argument_type = Vector; - using return_type = double; + /// For serialization + EvaluationFunctor() {} /// Constructor with interval [a,b] EvaluationFunctor(size_t N, double x) @@ -85,7 +96,7 @@ class Basis { }; /** - * VectorEvaluationFunctor at a given x, applied to MatrixMN. + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M * corresponding functions at x, possibly with Jacobian wrpt the parameters. @@ -93,21 +104,20 @@ class Basis { template class VectorEvaluationFunctor : protected EvaluationFunctor { protected: - typedef Eigen::Matrix VectorM; - typedef Eigen::Matrix MatrixMN; - typedef Eigen::Matrix Jacobian; + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + void calculateJacobian() { - typedef Eigen::Matrix MatrixM; + using MatrixM = Eigen::Matrix; H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // Used by FunctorizedFactor - using argument_type = Matrix; - using return_type = Vector; + /// For serialization + VectorEvaluationFunctor() {} /// Default Constructor VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { @@ -121,14 +131,14 @@ class Basis { } /// M-dimensional evaluation - VectorM apply(const MatrixMN& F, + VectorM apply(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F * this->weights_.transpose(); + return F.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const MatrixMN& F, + VectorM operator()(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { return apply(F, H); } @@ -139,10 +149,9 @@ class Basis { * given row i, with 0 - class ComponentEvaluationFunctor : public EvaluationFunctor { + class VectorComponentFunctor : public EvaluationFunctor { protected: - typedef Eigen::Matrix MatrixMN; - typedef Eigen::Matrix Jacobian; + using Jacobian = Eigen::Matrix; size_t rowIndex_; Jacobian H_; void calculateJacobian(size_t N) { @@ -152,38 +161,37 @@ class Basis { } public: - // Used by FunctorizedFactor - using argument_type = Matrix; - using return_type = double; + /// For serialization + VectorComponentFunctor() {} /// Construct with row index - ComponentEvaluationFunctor(size_t N, size_t i, double x) + VectorComponentFunctor(size_t N, size_t i, double x) : EvaluationFunctor(N, x), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - ComponentEvaluationFunctor(size_t N, size_t i, double x, double a, double b) + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) : EvaluationFunctor(N, x, a, b), rowIndex_(i) { calculateJacobian(N); } /// Calculate component of component rowIndex_ of F - double apply(const MatrixMN& F, + double apply(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { if (H) *H = H_; return F.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar - double operator()(const MatrixMN& F, + double operator()(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { return apply(F, H); } }; /** - * Manifold EvaluationFunctor at a given x, applied to MatrixMN. + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M * corresponding functions at x, possibly with Jacobian wrpt the parameters. @@ -196,9 +204,8 @@ class Basis { using Base = VectorEvaluationFunctor; public: - // Used by FunctorizedFactor - using argument_type = Matrix; - using return_type = T; + /// For serialization + ManifoldEvaluationFunctor() {} /// Default Constructor ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} @@ -208,7 +215,7 @@ class Basis { : Base(N, x, a, b) {} /// Manifold evaluation - T apply(const typename Base::MatrixMN& F, + T apply(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(F, H); @@ -227,18 +234,21 @@ class Basis { } /// c++ sugar - T operator()(const typename Base::MatrixMN& F, + T operator()(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { return apply(F, H); // might call apply in derived } }; - /// Base class for functors below that calculates weights + /// Base class for functors below that calculate derivative weights class DerivativeFunctorBase { protected: typename DERIVED::Weights weights_; public: + /// For serialization + DerivativeFunctorBase() {} + DerivativeFunctorBase(size_t N, double x) : weights_(DERIVED::DerivativeWeights(N, x)) {} @@ -253,9 +263,8 @@ class Basis { /// Given values f at the Chebyshev points, predict derivative at x class DerivativeFunctor : protected DerivativeFunctorBase { public: - // Used by FunctorizedFactor - using argument_type = typename DERIVED::Parameters; - using return_type = double; + /// For serialization + DerivativeFunctor() {} DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} @@ -274,26 +283,25 @@ class Basis { } }; - /// Vector interpolation, e.g. given 3*N matrix yields 3-vector + /// Compute derivative vector of ParameterMatrix at specified point. template class VectorDerivativeFunctor : protected DerivativeFunctorBase { protected: - typedef Eigen::Matrix VectorM; - typedef Eigen::Matrix MatrixMN; - typedef Eigen::Matrix Jacobian; + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + void calculateJacobian() { - typedef Eigen::Matrix MatrixM; + using MatrixM = Eigen::Matrix; H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); } public: - // Used by FunctorizedFactor - using argument_type = Matrix; - using return_type = Vector; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// For serialization + VectorDerivativeFunctor() {} + /// Default Constructor VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { calculateJacobian(); @@ -305,29 +313,30 @@ class Basis { calculateJacobian(); } - VectorM apply(const MatrixMN& F, + VectorM apply(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F * this->weights_.transpose(); + return F.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const MatrixMN& F, OptionalJacobian H = - boost::none) const { + VectorM operator()( + const ParameterMatrix& F, + OptionalJacobian H = boost::none) const { return apply(F, H); } }; /** * Given M*N Matrix of M-vectors at N Chebyshev points, predict derivative for - * given row i, with 0 class ComponentDerivativeFunctor : protected DerivativeFunctorBase { protected: - typedef Eigen::Matrix MatrixMN; - typedef Eigen::Matrix Jacobian; + using Jacobian = Eigen::Matrix; size_t rowIndex_; Jacobian H_; + void calculateJacobian(size_t N) { H_.setZero(1, M * N); for (int j = 0; j < this->weights_.size(); j++) @@ -335,9 +344,8 @@ class Basis { } public: - // Used by FunctorizedFactor - using argument_type = Matrix; - using return_type = double; + /// For serialization + ComponentDerivativeFunctor() {} /// Construct with row index ComponentDerivativeFunctor(size_t N, size_t i, double x) @@ -351,13 +359,13 @@ class Basis { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F - double apply(const MatrixMN& F, + double apply(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { if (H) *H = H_; return F.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar - double operator()(const MatrixMN& F, + double operator()(const ParameterMatrix& F, OptionalJacobian H = boost::none) const { return apply(F, H); } diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index f11ec9e53f..2ed4335d7c 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -18,74 +18,204 @@ #include +#include + namespace gtsam { /** - * Factor for BASIS evaluation. + * Factor for scalar BASIS evaluation. * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -using PredictFactor = - FunctorizedFactor; +class EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + EvaluationFactor(Key key, const 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() {} +}; /** - * Factor for BASIS derivative evaluation. + * Prior factor for BASIS vector evaluation on ParameterMatrix of size (M,N). * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. */ -template -using DerivativeFactor = - FunctorizedFactor; +template +class VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + 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(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(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; /** - * Prior factor for BASIS vector evaluation on Matrix of size (M, N_ORDER). + * Create a measurement on any scalar entry (chosen in the constructor) of a + * P-dimensional vector computed by a basis parameterization. * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector. + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. */ -template -using VectorPrior = FunctorizedFactor< - typename BASIS::template VectorEvaluationFunctor::return_type, - typename BASIS::template VectorEvaluationFunctor::argument_type>; +template +class VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + 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

(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

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; /** * Prior factor for BASIS Manifold evaluation on type T. * @param BASIS: The basis class to use e.g. Chebyshev2 * @param T: Object type which is synthesized by the functor. */ -template -using TypePrior = FunctorizedFactor< - typename BASIS::template ManifoldEvaluationFunctor::return_type, - typename BASIS::template ManifoldEvaluationFunctor::argument_type>; +template +class ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::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(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(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; /** - * Prior factor for BASIS component evaluation on Matrix of size (M, N_ORDER). + * Factor for scalar BASIS derivative evaluation. * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the component. */ -template -using ComponentPrior = FunctorizedFactor< - typename BASIS::template ComponentEvaluationFunctor

::return_type, - typename BASIS::template ComponentEvaluationFunctor

::argument_type>; +template +class DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + 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() {} +}; /** - * Prior factor for BASIS vector derivative on Matrix of size (M, N_ORDER). + * Prior factor for BASIS vector derivative on ParameterMatrix of size (M,N). * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector derivative. */ -template -using VectorDerivativePrior = FunctorizedFactor< - typename BASIS::template VectorDerivativeFunctor::return_type, - typename BASIS::template VectorDerivativeFunctor::argument_type>; +template +class VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + 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() {} +}; /** - * Prior factor for BASIS component derivative on Matrix of size (M, N_ORDER). + * Prior factor for BASIS component derivative on ParameterMatrix of size (M,N). * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the component derivative. + * @param P: Size of the control component derivative. */ -template -using ComponentDerivativePrior = FunctorizedFactor< - typename BASIS::template ComponentDerivativeFunctor

::return_type, - typename BASIS::template ComponentDerivativeFunctor

::argument_type>; +template +class ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + 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 diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 0000000000..aea8662b5c --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Chebyshev2Basis::Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { + Weights Ux(N); + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Chebyshev1Basis::Weights Chebyshev1Basis::CalculateWeights(size_t N, double x) { + Weights Tx(1, N); + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 4a2af0635b..6733d141e6 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev.h * @brief Chebyshev basis decompositions @@ -24,24 +35,14 @@ namespace gtsam { * functions. In this sense, they are like the sines and cosines of the Fourier * basis. */ -class Chebyshev2Basis : public Basis { - public: +struct Chebyshev2Basis : Basis { using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) */ - static Weights CalculateWeights(size_t N, double x) { - Weights Ux(N); - Ux(0) = 1; - Ux(1) = 2 * x; - for (size_t i = 2; i < N; i++) { - // instead of cos(i*acos(x)), this recurrence is much faster - Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); - } - return Ux; - } + static Weights CalculateWeights(size_t N, double x); }; // Chebyshev2Basis @@ -51,39 +52,29 @@ class Chebyshev2Basis : public Basis { * These are typically denoted with the symbol T_n, where n is the degree. * The parameter N is the number of coefficients, i.e., N = n+1. */ -class Chebyshev1Basis : public Basis { - public: +struct Chebyshev1Basis : Basis { using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; /** - * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) */ - static Weights CalculateWeights(size_t N, double x) { - Weights Tx(1, N); - Tx(0) = 1; - Tx(1) = x; - for (size_t i = 2; i < N; i++) { - // instead of cos(i*acos(x)), this recurrence is much faster - Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); - } - return Tx; - } + static Weights CalculateWeights(size_t N, double x); /// Create a Chebyshev derivative function of Parameters class Derivative { protected: - size_t N_; - - public: // From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) // I.e. the derivative fo a first kind cheb is just a second kind cheb // So, we define a second kind basis here of order N-1 // Note that it has one less weight: typename Chebyshev2Basis::Weights weights_; + size_t N_; + + public: Derivative(size_t N, double x) - : N_(N), weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)) { + : weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)), N_(N) { // after the above init, weights_ contains the U_{n-1} values for n=1..N-1 // Note there is no value for n=0. But we need n*U{n-1}, so correct: for (size_t n = 1; n < N; n++) { @@ -92,7 +83,7 @@ class Chebyshev1Basis : public Basis { } double operator()(const Parameters &c) { - // The Parameters pertain to 1st kind chebyshevs up to order N-1 + // The Parameters pertain to 1st kind chebs up to order N-1 // But of course the first one (order 0) is constant, so omit that weight return (weights_ * c.block(1, 0, N_ - 1, 1))(0); } diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1386a49830..e1c5b5cbac 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.h * @brief Chebyshev parameterizations on Chebyshev points of second kind. @@ -24,28 +35,14 @@ namespace gtsam { * Note that N here, the #points, is one less than N from Trefethen00book * (pg.42) */ -class Chebyshev2 : public Basis { +struct Chebyshev2 : public Basis { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - public: using Base = Basis; using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; - /** - * A matrix of M*N values at the Chebyshev points, where M is the dimension of - * T template argument T: the type you would like to EvaluationFunctor using - * polynomial interpolation template argument N: the number of Chebyshev - * points of the second kind - */ - template - struct ParameterMatrix { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // First dimension is: traits::dimension - typedef Eigen::Matrix type; - }; - /// Specific Chebyshev point static double Point(size_t N, int j) { assert(j >= 0 && size_t(j) < N); @@ -85,14 +82,13 @@ class Chebyshev2 : public Basis { * values of the function f at the Chebyshev points. As such, for a given x we * obtain a linear map from parameter vectors f to interpolated values f(x). * Optional [a,b] interval can be specified as well. - * - * Please refer to Trefethen13chapters p 35, formula 5.13 */ static Weights CalculateWeights(size_t N, double x, double a = -1, double b = 1); /** * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. */ static Weights DerivativeWeights(size_t N, double x, double a = -1, double b = 1); @@ -128,14 +124,14 @@ class Chebyshev2 : public Basis { * Create matrix of values at Chebyshev points given vector-valued function. */ template - static Matrix matrix( - boost::function(double)> f, // - size_t N, double a = -1, double b = 1) { + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { Matrix Xmat(M, N); - for (size_t j = 0; j < N; j++) Xmat.col(j) = f(Point(N, j, a, b)); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } return Xmat; } -}; -// \ Chebyshev2 +}; // \ Chebyshev2 } // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 213d5a995e..f187597a4c 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -24,17 +24,16 @@ #pragma once +#include +#include #include #include #include -#include "Basis.h" -#include "BasisFactors.h" - namespace gtsam { /// For now, this is our sequence representation -typedef std::map Sequence; +using Sequence = std::map; /** * Class that does Fourier Decomposition via least squares @@ -42,8 +41,8 @@ typedef std::map Sequence; template class FitBasis { public: - typedef std::pair Sample; - typedef typename Basis::Parameters Parameters; + using Sample = std::pair; + using Parameters = typename Basis::Parameters; private: Parameters parameters_; @@ -55,9 +54,8 @@ class FitBasis { size_t N) { NonlinearFactorGraph graph; for (const Sample sample : sequence) { - auto functor = typename Basis::EvaluationFunctor(N, sample.first); - graph.emplace_shared>(0, sample.second, model, - functor); + graph.emplace_shared>( + 0, sample.second, model, N, sample.first); } return graph; } diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 0000000000..1157d58341 --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,218 @@ +/* ---------------------------------------------------------------------------- + + * 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 ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam T: the type you would like to evaluate using polynomial + * interpolation. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + * @param row: The Eigen (row) vector used to set the values. + */ + void setRow(size_t index, const Eigen::Matrix& row) { + matrix_.row(index) = row; + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + * @param row: The Eigen (column) vector used to set the values. + */ + void setCol(size_t index, const Eigen::Matrix& col) { + matrix_.col(index) = col; + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index bb987405fa..c1d8fbdd30 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -31,7 +31,7 @@ const size_t N = 3; //****************************************************************************** TEST(Chebyshev, Chebyshev1) { - typedef Chebyshev1Basis::EvaluationFunctor Synth; + using Synth = Chebyshev1Basis::EvaluationFunctor; Vector c(N); double x; c << 12, 3, 1; @@ -45,7 +45,7 @@ TEST(Chebyshev, Chebyshev1) { //****************************************************************************** TEST(Chebyshev, Chebyshev2) { - typedef Chebyshev2Basis::EvaluationFunctor Synth; + using Synth = Chebyshev2Basis::EvaluationFunctor; Vector c(N); double x; c << 12, 3, 1; @@ -58,7 +58,7 @@ TEST(Chebyshev, Chebyshev2) { } //****************************************************************************** -TEST(Chebyshev, Prediction) { +TEST(Chebyshev, Evaluation) { Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); Vector c(N); c << 3, 5, -12; @@ -82,9 +82,8 @@ TEST(Chebyshev, Expression) { x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; for (size_t i = 0; i < m; i++) { - auto functor = Chebyshev1Basis::EvaluationFunctor(N, t(i)); - graph.emplace_shared>(key, x(i), model, - functor); + graph.emplace_shared>(key, x(i), model, N, + t(i)); } // Solve @@ -141,12 +140,13 @@ TEST(Chebyshev, Decomposition) { //****************************************************************************** TEST(Chebyshev1, Derivative) { - typedef Chebyshev1Basis::Derivative D; + using D = Chebyshev1Basis::Derivative; + Vector c(N); - double x; c << 12, 3, 1; - // D[12 + 3*x + 2*x*x-1,x] = 3 + 4*x - x = -1.0; + + // The derivative should be D(12 + 3*x + 2*x*x-1, x) = 3 + 4*x + double x = -1.0; EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); x = -0.5; EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index e20c347981..bfb10fc38b 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -28,6 +28,8 @@ using namespace gtsam; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); +const size_t N = 32; + //****************************************************************************** TEST(Chebyshev2, Point) { static const int N = 5; @@ -99,20 +101,22 @@ TEST(Chebyshev2, InterpolateVector) { double t = 30, a = 0, b = 100; const size_t N = 3; // Create 2x3 matrix with Vectors at Chebyshev points - Matrix X = Matrix::Zero(2, 3); - X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + ParameterMatrix<2> X(N); + X.setRow(0, Chebyshev2::Points(N, a, b)); // slope 1 ramp // Check value Vector expected(2); expected << t, 0; Eigen::Matrix actualH(2, 2 * N); + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - boost::function f = boost::bind( + boost::function)> f = boost::bind( &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); - Matrix numericalH = numericalDerivative11(f, X); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -184,7 +188,6 @@ double fprime(double x) { //****************************************************************************** TEST(Chebyshev2, CalculateWeights) { - const size_t N = 32; Eigen::Matrix fvals(N); for (size_t i = 0; i < N; i++) { fvals(i) = f(Chebyshev2::Point(N, i)); @@ -197,7 +200,6 @@ TEST(Chebyshev2, CalculateWeights) { } TEST(Chebyshev2, CalculateWeights2) { - const size_t N = 32; double a = 0, b = 10, x1 = 7, x2 = 4.12; Eigen::Matrix fvals(N); @@ -209,33 +211,33 @@ TEST(Chebyshev2, CalculateWeights2) { EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); Chebyshev2::Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); - double expected2 = 185.454784; + double expected2 = f(x2); // 185.454784 double actual2 = weights2 * fvals; EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); } TEST(Chebyshev2, DerivativeWeights) { - const size_t N = 32; - Eigen::Matrix fvals(N); for (size_t i = 0; i < N; i++) { fvals(i) = f(Chebyshev2::Point(N, i)); } - double x1 = 0.7, x2 = -0.376; + double x1 = 0.7, x2 = -0.376, x3 = 0.0; Chebyshev2::Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); - EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); Chebyshev2::Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); - EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); - // test if derivative calculation and cheb point is correct - double x3 = Chebyshev2::Point(N, 3); Chebyshev2::Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); - EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Chebyshev2::Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); } TEST(Chebyshev2, DerivativeWeights2) { - const size_t N = 32; double x1 = 5, x2 = 4.12, a = 0, b = 10; Eigen::Matrix fvals(N); @@ -263,14 +265,14 @@ TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); Chebyshev2::Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; Chebyshev2::Weights actual = Chebyshev2::DerivativeWeights(N6, x1); - EXPECT(assert_equal(expected, actual, 1e-4)); + EXPECT(assert_equal(expected, actual, 1e-12)); double a = -3, b = 8, x2 = 5.05; Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); Chebyshev2::Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; Chebyshev2::Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); - EXPECT(assert_equal(expected1, actual1, 1e-4)); + EXPECT(assert_equal(expected1, actual1, 1e-12)); } //****************************************************************************** @@ -345,28 +347,63 @@ TEST(Chebyshev2, Derivative6_03) { TEST(Chebyshev2, VectorDerivativeFunctor) { const size_t N = 3, M = 2; const double x = 0.2; - typedef Chebyshev2::VectorDerivativeFunctor VecD; + using VecD = Chebyshev2::VectorDerivativeFunctor; VecD fx(N, x, 0, 3); - Matrix X = Matrix::Zero(M, N); + ParameterMatrix X(N); Matrix actualH(M, M * N); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( boost::bind(&VecD::operator(), fx, _1, boost::none), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + //****************************************************************************** // Test ComponentDerivativeFunctor TEST(Chebyshev2, ComponentDerivativeFunctor) { - const size_t N = 7, M = 3; + const size_t N = 6, M = 2; const double x = 0.2; - typedef Chebyshev2::ComponentDerivativeFunctor CompFunc; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; CompFunc fx(N, row, x, 0, 3); - Matrix X = Matrix::Zero(M, 7); + ParameterMatrix X(N); Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11( + + Matrix expectedH = numericalDerivative11, M * N>( boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index 9f853ec1bd..b6cae7a260 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -12,8 +12,8 @@ /** * @file testFourier.cpp * @date July 4, 2020 - * @author Frank Dellaert - * @brief unit tests for Fourier Basis Decompositions + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions */ #include @@ -83,8 +83,8 @@ TEST(Basis, Manual) { graph.add(linearFactor); // Create factor to predict value at x - PredictFactor predictFactor( - key, desiredValue, model, FourierBasis::EvaluationFunctor(N, x)); + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); // Check expression Jacobians EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); @@ -102,7 +102,7 @@ TEST(Basis, Manual) { } //****************************************************************************** -TEST(Basis, PredictFactor) { +TEST(Basis, EvaluationFactor) { // Check fitting a function with a 7-parameter Fourier basis // Create linear factor graph @@ -110,8 +110,8 @@ TEST(Basis, PredictFactor) { Key key(1); for (size_t i = 0; i < 16; i++) { double x = i * M_PI / 8, desiredValue = TestFunction(x); - graph.emplace_shared>( - key, desiredValue, model, FourierBasis::EvaluationFunctor(7, x)); + graph.emplace_shared>(key, desiredValue, + model, 7, x); } // Solve FourierFactorGraph @@ -184,9 +184,10 @@ TEST(Basis, VecDerivativeFunctor) { double h = 2 * M_PI / 16; Vector2 dotShape(0.5556, -0.8315); // at h/2 DotShape dotShapeFunction(N, h / 2); - Matrix23 theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) - .finished() - .transpose(); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); } @@ -202,45 +203,45 @@ TEST(Basis, VecDerivativeFunctor) { // we can simply express the coefficents c as c=inv(W(X))*f, which is a // generalized Fourier transform. That also means we can create factors with the // unknown f-values, as done manually below. -// TEST(Basis, PseudoSpectral) { -// // We will create a linear factor graph -// GaussianFactorGraph graph; - -// const size_t N = 3; -// const Key key(1); - -// // The correct values at X = {0.1,0.2,0.3} are simply W*c -// const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); -// const Matrix W = FourierBasis::WeightMatrix(N, X); -// const Vector expected = W * k3Coefficients; - -// // Check those values are indeed correct values of Fourier approximation -// using Eval = FourierBasis::EvaluationFunctor; -// EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); -// EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); -// EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); - -// // Calculate "inverse Fourier transform" matrix -// const Matrix invW = W.inverse(); - -// // At 16 different samples points x, add a factor on fExpr -// for (size_t i = 0; i < 16; i++) { -// const double x = i * M_PI / 8; -// const double desiredValue = TestFunction(x); - -// // Manual JacobianFactor -// Matrix A(1, 3); -// A << 1, cos(x), sin(x); -// Vector b(1); -// b << desiredValue; -// JacobianFactor linearFactor(key, A * invW, b); -// graph.add(linearFactor); -// } - -// // Solve linear graph -// VectorValues actual = graph.optimize(); -// EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); -// } +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} //****************************************************************************** int main() { diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 0000000000..68147ec21a --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // setRow + params.setRow(0, Vector::Ones(N)); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // setCol + params.setCol(2, Vector::Ones(M)); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d053c84222..ad6f9fc29c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -236,6 +236,137 @@ virtual class GenericValue : gtsam::Value { void serializable() const; }; +//************************************************************************* +// basis +//************************************************************************* + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); + + // TODO need support for nested classes + // class Derivative { + // Derivative(size_t N, double x); + // }; +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + MatrixType matrix() const; + + void print(const string& s="") const; +}; + +typedef gtsam::ParameterMatrix<1> StateVector; +typedef gtsam::ParameterMatrix<2> BicycleControlMatrix; +typedef gtsam::ParameterMatrix<3> PlanarStateMatrix; +typedef gtsam::ParameterMatrix<4> QuadControlMatrix; +typedef gtsam::ParameterMatrix<6> ControlMatrix; +typedef gtsam::ParameterMatrix<12> StateMatrix; + +#include + +template +class EvaluationFactor : gtsam::NonlinearFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +class VectorEvaluationFactor : gtsam::NonlinearFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +class VectorComponentFactor : gtsam::NonlinearFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +class ManifoldEvaluationFactor : gtsam::NonlinearFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + + //************************************************************************* // geometry //************************************************************************* From c75c6689a8183a0224f5335477131e4e6a31fdbf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 4 Jun 2021 13:30:46 -0400 Subject: [PATCH 05/36] set Eigen Unsupported to ON by default --- cmake/HandleEigen.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index b21d16885f..eeecf52b98 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -6,7 +6,7 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us if(NOT GTSAM_USE_SYSTEM_EIGEN) # This option only makes sense if using the embedded copy of Eigen, it is # used to decide whether to *install* the "unsupported" module: - option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) + option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" ON) endif() # Switch for using system Eigen or GTSAM-bundled Eigen From f4d8f4220e38ac08957b76adb396b883de3b5f8e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 4 Jun 2021 17:11:44 -0400 Subject: [PATCH 06/36] Added more tests --- .../nonlinear/tests/testFunctorizedFactor.cpp | 140 +++++++++++++++++- 1 file changed, 137 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e7229..14a14fc197 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; From d456e807d00d3130741c67eeb6772be791db46e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Jun 2021 19:59:06 -0400 Subject: [PATCH 07/36] move factor wrapping after NoiseModelFactor --- gtsam/gtsam.i | 132 ++++++++++++++++++++++++-------------------------- 1 file changed, 63 insertions(+), 69 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index ad6f9fc29c..dcf33444fe 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -289,7 +289,7 @@ class Chebyshev2 { #include -template +template class ParameterMatrix { ParameterMatrix(const size_t N); ParameterMatrix(const Matrix& matrix); @@ -299,74 +299,6 @@ class ParameterMatrix { void print(const string& s="") const; }; -typedef gtsam::ParameterMatrix<1> StateVector; -typedef gtsam::ParameterMatrix<2> BicycleControlMatrix; -typedef gtsam::ParameterMatrix<3> PlanarStateMatrix; -typedef gtsam::ParameterMatrix<4> QuadControlMatrix; -typedef gtsam::ParameterMatrix<6> ControlMatrix; -typedef gtsam::ParameterMatrix<12> StateMatrix; - -#include - -template -class EvaluationFactor : gtsam::NonlinearFactor { - EvaluationFactor(); - EvaluationFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - double x); - EvaluationFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - double x, double a, double b); -}; - -template -class VectorEvaluationFactor : gtsam::NonlinearFactor { - VectorEvaluationFactor(); - VectorEvaluationFactor(gtsam::Key key, const Vector& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x); - VectorEvaluationFactor(gtsam::Key key, const Vector& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x, double a, double b); -}; - -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D3; -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D4; -typedef gtsam::VectorEvaluationFactor - VectorEvaluationFactorChebyshev2D12; - -template -class VectorComponentFactor : gtsam::NonlinearFactor { - VectorComponentFactor(); - VectorComponentFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - size_t i, double x); - VectorComponentFactor(gtsam::Key key, const double z, - const gtsam::noiseModel::Base* model, const size_t N, - size_t i, double x, double a, double b); -}; - -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D3; -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D4; -typedef gtsam::VectorComponentFactor - VectorComponentFactorChebyshev2D12; - -template -class ManifoldEvaluationFactor : gtsam::NonlinearFactor { - ManifoldEvaluationFactor(); - ManifoldEvaluationFactor(gtsam::Key key, const T& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x); - ManifoldEvaluationFactor(gtsam::Key key, const T& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x, double a, double b); -}; - - //************************************************************************* // geometry //************************************************************************* @@ -3667,6 +3599,68 @@ class ScenarioRunner { Matrix estimateNoiseCovariance(size_t N) const; }; + +#include + +template +class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + + //************************************************************************* // Utilities //************************************************************************* From e8c96a0a2a3abe0892e5506188ba030ae53aabc6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Jun 2021 23:45:38 -0400 Subject: [PATCH 08/36] address review comments --- gtsam/basis/Basis.h | 70 ++++++++++++++++------- gtsam/basis/BasisFactors.h | 23 +++++++- gtsam/basis/Chebyshev.cpp | 6 +- gtsam/basis/Chebyshev.h | 8 +-- gtsam/basis/Chebyshev2.cpp | 22 ++++--- gtsam/basis/Chebyshev2.h | 3 +- gtsam/basis/FitBasis.h | 8 +-- gtsam/basis/Fourier.h | 15 ++++- gtsam/basis/ParameterMatrix.h | 2 +- gtsam/basis/tests/testChebyshev2.cpp | 31 +++++----- gtsam/basis/tests/testFourier.cpp | 17 +++--- gtsam/basis/tests/testParameterMatrix.cpp | 2 +- 12 files changed, 137 insertions(+), 70 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index f1747f19aa..a6eb6f1635 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -12,7 +12,7 @@ /** * @file Basis.h * @brief Compute an interpolating basis - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ @@ -22,23 +22,49 @@ #include #include -#include #include +#include /** - * Concept of a Basis: - * - type Weights, Parameters - * - CalculateWeights(size_t N, double a=default, double b=default) + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which teh function is evaluated, and `p` are + * parameters, e.g., coefficients used to weight the different basis functions. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - type `Weights`: vector of partial derivatives of `f(x;p)` wrt `p` + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` */ namespace gtsam { +using Weights = Eigen::Matrix; /* 1xN vector */ + /// CRTP Base class for function bases template class Basis { public: - /// Call weights for all x in vector, returns M*N matrix where M is the size - /// of the vector X. + /** + * Call weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis function. + */ static Matrix WeightMatrix(size_t N, const Vector& X) { Matrix W(X.size(), N); for (int i = 0; i < X.size(); i++) @@ -46,8 +72,11 @@ class Basis { return W; } - /// Call weights for all x in vector, with interval [a,b] - /// Returns M*N matrix where M is the size of the vector X. + /** + * Call weights for all x in vector X, with interval [a,b]. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis function. + */ static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { Matrix W(X.size(), N); for (int i = 0; i < X.size(); i++) @@ -56,14 +85,15 @@ class Basis { } /** - * EvaluationFunctor at a given x, applied to Parameters. - * This functor is used to evaluate a parameterized function at a given scalar - * value x. When given a specific N*1 vector of Parameters, returns the scalar - * value of the function at x, possibly with Jacobian wrpt the parameters. + * EvaluationFunctor calculate f(x;p) at a given `x`, applied to Parameters + * `p`. This functor is used to evaluate a parameterized function at a given + * scalar value x. When given a specific N*1 vector of Parameters, returns the + * scalar value of the function at x, possibly with Jacobian wrpt the + * parameters. */ class EvaluationFunctor { protected: - typename DERIVED::Weights weights_; + Weights weights_; public: /// For serialization @@ -132,21 +162,21 @@ class Basis { /// M-dimensional evaluation VectorM apply(const ParameterMatrix& F, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = boost::none) const { if (H) *H = H_; return F.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()(const ParameterMatrix& F, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = boost::none) const { return apply(F, H); } }; /** - * Given M*N Matrix of M-vectors at N Chebyshev points, predict component for - * given row i, with 0 class VectorComponentFunctor : public EvaluationFunctor { @@ -235,7 +265,7 @@ class Basis { /// c++ sugar T operator()(const ParameterMatrix& F, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = boost::none) const { return apply(F, H); // might call apply in derived } }; @@ -243,7 +273,7 @@ class Basis { /// Base class for functors below that calculate derivative weights class DerivativeFunctorBase { protected: - typename DERIVED::Weights weights_; + Weights weights_; public: /// For serialization diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 2ed4335d7c..f0e730f3f2 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -16,9 +16,8 @@ #pragma once -#include - #include +#include namespace gtsam { @@ -34,10 +33,30 @@ class EvaluationFactor : public FunctorizedFactor { public: EvaluationFactor() {} + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The result of the functional evaluation. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The result of the functional evaluation. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ EvaluationFactor(Key key, const 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)) {} diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp index aea8662b5c..75012f5489 100644 --- a/gtsam/basis/Chebyshev.cpp +++ b/gtsam/basis/Chebyshev.cpp @@ -12,7 +12,7 @@ /** * @file Chebyshev.cpp * @brief Chebyshev basis decompositions - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ @@ -20,7 +20,7 @@ namespace gtsam { -Chebyshev2Basis::Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { Weights Ux(N); Ux(0) = 1; Ux(1) = 2 * x; @@ -31,7 +31,7 @@ Chebyshev2Basis::Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { return Ux; } -Chebyshev1Basis::Weights Chebyshev1Basis::CalculateWeights(size_t N, double x) { +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x) { Weights Tx(1, N); Tx(0) = 1; Tx(1) = x; diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 6733d141e6..0655a13c48 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -12,7 +12,7 @@ /** * @file Chebyshev.h * @brief Chebyshev basis decompositions - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ @@ -21,7 +21,7 @@ #include #include -#include +#include namespace gtsam { @@ -36,7 +36,6 @@ namespace gtsam { * basis. */ struct Chebyshev2Basis : Basis { - using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; /** @@ -53,7 +52,6 @@ struct Chebyshev2Basis : Basis { * The parameter N is the number of coefficients, i.e., N = n+1. */ struct Chebyshev1Basis : Basis { - using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; /** @@ -68,7 +66,7 @@ struct Chebyshev1Basis : Basis { // I.e. the derivative fo a first kind cheb is just a second kind cheb // So, we define a second kind basis here of order N-1 // Note that it has one less weight: - typename Chebyshev2Basis::Weights weights_; + Weights weights_; size_t N_; diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 9089eb91e7..8e59001d5d 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.cpp * @brief Chebyshev parameterizations on Chebyshev points of second kind - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ @@ -9,8 +20,7 @@ namespace gtsam { -Chebyshev2::Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, - double b) { +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); @@ -49,8 +59,7 @@ Chebyshev2::Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, return weights / d; } -Chebyshev2::Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, - double b) { +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weightDerivatives(N); @@ -171,8 +180,7 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, return D / ((b - a) / 2.0); } -Chebyshev2::Weights Chebyshev2::IntegrationWeights(size_t N, double a, - double b) { +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { // Allocate space for weights Weights weights(N); size_t K = N - 1, // number of intervals between N points diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index e1c5b5cbac..ce3565923a 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -16,7 +16,7 @@ * pseudo-spectral optimization, i.e. we don't decompose into basis * functions, rather estimate function parameters that enforce function * nodes at Chebyshev points. - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ @@ -39,7 +39,6 @@ struct Chebyshev2 : public Basis { EIGEN_MAKE_ALIGNED_OPERATOR_NEW using Base = Basis; - using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index f187597a4c..d16422dab1 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -12,7 +12,7 @@ /** * @file FitBasis.h * @date July 4, 2020 - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @brief Fit a Basis using least-squares */ @@ -32,7 +32,7 @@ namespace gtsam { -/// For now, this is our sequence representation +/// Our sequence representation is a map of {x: y} values where y = f(x) using Sequence = std::map; /** @@ -54,8 +54,8 @@ class FitBasis { size_t N) { NonlinearFactorGraph graph; for (const Sample sample : sequence) { - graph.emplace_shared>( - 0, sample.second, model, N, sample.first); + graph.emplace_shared>(0, sample.second, model, N, + sample.first); } return graph; } diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index c07ce275e0..bd8d2e94da 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 Fourier.h * @brief Fourier decomposition, see e.g. - * http://mathworld.wolfram.com/FourierSeries.html - * @author Varun Agrawal + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert * @date July 4, 2020 */ diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index 1157d58341..a9d778ffda 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -13,7 +13,7 @@ * @file ParamaterMatrix.h * @brief Define ParameterMatrix class which is used to store values at * interpolation points. - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @date September 21, 2020 */ diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index bfb10fc38b..2e54db07dd 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -193,8 +193,8 @@ TEST(Chebyshev2, CalculateWeights) { fvals(i) = f(Chebyshev2::Point(N, i)); } double x1 = 0.7, x2 = -0.376; - Chebyshev2::Weights weights1 = Chebyshev2::CalculateWeights(N, x1); - Chebyshev2::Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); } @@ -207,10 +207,10 @@ TEST(Chebyshev2, CalculateWeights2) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } - Chebyshev2::Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); - Chebyshev2::Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); double expected2 = f(x2); // 185.454784 double actual2 = weights2 * fvals; EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); @@ -222,18 +222,18 @@ TEST(Chebyshev2, DerivativeWeights) { fvals(i) = f(Chebyshev2::Point(N, i)); } double x1 = 0.7, x2 = -0.376, x3 = 0.0; - Chebyshev2::Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); - Chebyshev2::Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); - Chebyshev2::Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); // test if derivative calculation and cheb point is correct double x4 = Chebyshev2::Point(N, 3); - Chebyshev2::Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); } @@ -245,15 +245,15 @@ TEST(Chebyshev2, DerivativeWeights2) { fvals(i) = f(Chebyshev2::Point(N, i, a, b)); } - Chebyshev2::Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); - Chebyshev2::Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); // test if derivative calculation and cheb point is correct double x3 = Chebyshev2::Point(N, 3, a, b); - Chebyshev2::Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); } @@ -263,15 +263,14 @@ TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { const size_t N6 = 6; double x1 = 0.311; Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); - Chebyshev2::Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; - Chebyshev2::Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); EXPECT(assert_equal(expected, actual, 1e-12)); double a = -3, b = 8, x2 = 5.05; Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); - Chebyshev2::Weights expected1 = - Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; - Chebyshev2::Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); EXPECT(assert_equal(expected1, actual1, 1e-12)); } diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index b6cae7a260..fa3482ab7d 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -53,6 +53,9 @@ TEST(Basis, BasisEvaluationFunctorDerivative) { FourierBasis::EvaluationFunctor fx(3, 0); EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** @@ -89,11 +92,11 @@ TEST(Basis, Manual) { // Check expression Jacobians EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); - auto gaussianFactor = predictFactor.linearize(values); - auto jacobianFactor = - boost::dynamic_pointer_cast(gaussianFactor); - CHECK(jacobianFactor); // makes sure it's indeed a JacobianFactor - EXPECT(assert_equal(linearFactor, *jacobianFactor, 1e-9)); + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); } // Solve linear graph @@ -160,11 +163,11 @@ double proxy(double x) { TEST(Basis, Derivative7) { // Check Derivative evaluation at point x=0.2 - // calculate expected values by numerical derivative of synthesis + // Calculate expected values by numerical derivative of proxy. const double x = 0.2; Matrix numeric_dTdx = numericalDerivative11(proxy, x); - // Calculate derivatives at Chebyshev points using D3, interpolate + // Calculate derivatives at Chebyshev points using D7, interpolate Matrix D7 = FourierBasis::DifferentiationMatrix(7); Vector derivativeCoefficients = D7 * k7Coefficients; FourierBasis::EvaluationFunctor fx(7, x); diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp index 68147ec21a..0d41248bc5 100644 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -12,7 +12,7 @@ /** * @file testParameterMatrix.cpp * @date Sep 22, 2020 - * @author Varun Agrawal + * @author Varun Agrawal, Frank Dellaert * @brief Unit tests for ParameterMatrix.h */ From 1dd2004813cb29c59395d628168d1737d1664c11 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 13 Jun 2021 12:46:16 -0400 Subject: [PATCH 09/36] Fix minor bug in wrapper --- gtsam/gtsam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 5950bb4da7..ed4f52de26 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -294,7 +294,7 @@ class ParameterMatrix { ParameterMatrix(const size_t N); ParameterMatrix(const Matrix& matrix); - MatrixType matrix() const; + Matrix matrix() const; void print(const string& s="") const; }; From b7963709ef867390d7aa50cebb773ca738d1236b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Jun 2021 23:48:02 -0400 Subject: [PATCH 10/36] Add GTSAM_EXPORT to all classes --- gtsam/basis/Basis.h | 2 +- gtsam/basis/BasisFactors.h | 14 +++++++------- gtsam/basis/Chebyshev2.h | 3 ++- gtsam/basis/Fourier.h | 3 +-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index a6eb6f1635..b7acc1f8e7 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -58,7 +58,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ /// CRTP Base class for function bases template -class Basis { +class GTSAM_EXPORT Basis { public: /** * Call weights for all x in vector X. diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index f0e730f3f2..aed9447330 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -26,7 +26,7 @@ namespace gtsam { * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -class EvaluationFactor : public FunctorizedFactor { +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; @@ -70,7 +70,7 @@ class EvaluationFactor : public FunctorizedFactor { * @param M: Size of the evaluated state vector. */ template -class VectorEvaluationFactor +class GTSAM_EXPORT VectorEvaluationFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -105,7 +105,7 @@ class VectorEvaluationFactor * where N is the degree and i is the component index. */ template -class VectorComponentFactor +class GTSAM_EXPORT VectorComponentFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -134,7 +134,7 @@ class VectorComponentFactor * @param T: Object type which is synthesized by the functor. */ template -class ManifoldEvaluationFactor +class GTSAM_EXPORT ManifoldEvaluationFactor : public FunctorizedFactor::dimension>> { private: using Base = FunctorizedFactor::dimension>>; @@ -162,7 +162,7 @@ class ManifoldEvaluationFactor * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -class DerivativeFactor +class GTSAM_EXPORT DerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; @@ -187,7 +187,7 @@ class DerivativeFactor * @param M: Size of the evaluated state vector derivative. */ template -class VectorDerivativeFactor +class GTSAM_EXPORT VectorDerivativeFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; @@ -215,7 +215,7 @@ class VectorDerivativeFactor * @param P: Size of the control component derivative. */ template -class ComponentDerivativeFactor +class GTSAM_EXPORT ComponentDerivativeFactor : public FunctorizedFactor> { private: using Base = FunctorizedFactor>; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index ce3565923a..dd2c5320ac 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -35,7 +35,8 @@ namespace gtsam { * Note that N here, the #points, is one less than N from Trefethen00book * (pg.42) */ -struct Chebyshev2 : public Basis { +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using Base = Basis; diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index bd8d2e94da..2436ffe44b 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -24,9 +24,8 @@ namespace gtsam { /// Fourier basis -class FourierBasis : public Basis { +class GTSAM_EXPORT FourierBasis : public Basis { public: - using Weights = Eigen::Matrix; using Parameters = Eigen::Matrix; using DiffMatrix = Eigen::Matrix; From 97f600fad9253160481b751430c0787ca1b2ce83 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 27 Jul 2021 09:29:31 -0400 Subject: [PATCH 11/36] wrapper interface files --- gtsam/basis/basis.i | 62 ++++++++++++++++++++++++++++ python/gtsam/preamble/basis.h | 12 ++++++ python/gtsam/specializations/basis.h | 12 ++++++ 3 files changed, 86 insertions(+) create mode 100644 gtsam/basis/basis.i create mode 100644 python/gtsam/preamble/basis.h create mode 100644 python/gtsam/specializations/basis.h diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 0000000000..2751529c0e --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,62 @@ +//************************************************************************* +// basis +//************************************************************************* + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); + + // TODO need support for nested classes + // class Derivative { + // Derivative(size_t N, double x); + // }; +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s="") const; +}; diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 0000000000..a34e730580 --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ \ No newline at end of file diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ \ No newline at end of file From 09c5e5eca49ef6cf23b9fd8c27538fc0f7105619 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 27 Jul 2021 11:09:50 -0400 Subject: [PATCH 12/36] address review comments --- gtsam/basis/Basis.h | 6 +++--- gtsam/basis/FitBasis.h | 5 +++-- gtsam/basis/tests/testChebyshev.cpp | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index b7acc1f8e7..8231b44824 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -30,8 +30,9 @@ * combination of `basis functions` such as the Fourier basis on SO(2) or a set * of Chebyshev polynomials on [-1,1]. * In the expression `f(x;p)` the variable `x` is - * the continuous argument at which teh function is evaluated, and `p` are - * parameters, e.g., coefficients used to weight the different basis functions. + * the continuous argument at which the function is evaluated, and `p` are + * parameter which are coefficients of the different basis functions, + * e.g. p = (4, 3, 2) => 4 + 3x + 2x^2 for a polynomial. * However, different parameterizations are also possible. * The `Basis` class below defines a number of functors that can be used to @@ -47,7 +48,6 @@ * i.e., we expect it to define the following types and methods: - type `Parameters`: the parameters `p` in f(x;p) - - type `Weights`: vector of partial derivatives of `f(x;p)` wrt `p` - `CalculateWeights(size_t N, double x, double a=default, double b=default)` - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` */ diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index d16422dab1..5ee14ae6ad 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -34,6 +34,8 @@ namespace gtsam { /// Our sequence representation is a map of {x: y} values where y = f(x) using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; /** * Class that does Fourier Decomposition via least squares @@ -41,7 +43,6 @@ using Sequence = std::map; template class FitBasis { public: - using Sample = std::pair; using Parameters = typename Basis::Parameters; private: @@ -78,7 +79,7 @@ class FitBasis { } /// Return Fourier coefficients - Parameters parameters() { return parameters_; } + Parameters parameters() const { return parameters_; } }; } // namespace gtsam diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index c1d8fbdd30..9f5863567c 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -145,7 +145,7 @@ TEST(Chebyshev1, Derivative) { Vector c(N); c << 12, 3, 1; - // The derivative should be D(12 + 3*x + 2*x*x-1, x) = 3 + 4*x + // The derivative should be D(12 + 3*x + 2*x*x, x) = 3 + 4*x double x = -1.0; EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); x = -0.5; From df16905a762373aea895033ecb3c1670893f7767 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 27 Jul 2021 11:10:09 -0400 Subject: [PATCH 13/36] namespace boost::placeholders --- gtsam/basis/tests/testChebyshev2.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 2e54db07dd..afeccce87c 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace gtsam; +using namespace boost::placeholders; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); From 44953f40ec71abe266f58827edcc7dd951cee443 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jul 2021 11:58:54 -0400 Subject: [PATCH 14/36] make Cheb1 and Cheb2 consistent with CRTP concepts --- gtsam/basis/Chebyshev.cpp | 56 ++++++++++++++++++- gtsam/basis/Chebyshev.h | 83 ++++++++++++++++++----------- gtsam/basis/ParameterMatrix.h | 3 +- gtsam/basis/tests/testChebyshev.cpp | 81 +++++++++++++++++++++++----- 4 files changed, 175 insertions(+), 48 deletions(-) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp index 75012f5489..76aa0eb5f6 100644 --- a/gtsam/basis/Chebyshev.cpp +++ b/gtsam/basis/Chebyshev.cpp @@ -20,8 +20,28 @@ namespace gtsam { -Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { Weights Ux(N); + + x = scale(x, a, b, -1, 1); + Ux(0) = 1; Ux(1) = 2 * x; for (size_t i = 2; i < N; i++) { @@ -30,9 +50,31 @@ Weights Chebyshev2Basis::CalculateWeights(size_t N, double x) { } return Ux; } +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } -Weights Chebyshev1Basis::CalculateWeights(size_t N, double x) { + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + Tx(0) = 1; Tx(1) = x; for (size_t i = 2; i < N; i++) { @@ -42,4 +84,14 @@ Weights Chebyshev1Basis::CalculateWeights(size_t N, double x) { return Tx; } +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + } // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 0655a13c48..5084e633f9 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -39,9 +39,27 @@ struct Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; /** - * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). */ - static Weights CalculateWeights(size_t N, double x); + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); }; // Chebyshev2Basis @@ -54,38 +72,39 @@ struct Chebyshev2Basis : Basis { struct Chebyshev1Basis : Basis { using Parameters = Eigen::Matrix; + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + /** - * Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights */ - static Weights CalculateWeights(size_t N, double x); - - /// Create a Chebyshev derivative function of Parameters - class Derivative { - protected: - // From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) - // I.e. the derivative fo a first kind cheb is just a second kind cheb - // So, we define a second kind basis here of order N-1 - // Note that it has one less weight: - Weights weights_; - - size_t N_; - - public: - Derivative(size_t N, double x) - : weights_(Chebyshev2Basis::CalculateWeights(N - 1, x)), N_(N) { - // after the above init, weights_ contains the U_{n-1} values for n=1..N-1 - // Note there is no value for n=0. But we need n*U{n-1}, so correct: - for (size_t n = 1; n < N; n++) { - weights_(n - 1) *= double(n); - } - } - - double operator()(const Parameters &c) { - // The Parameters pertain to 1st kind chebs up to order N-1 - // But of course the first one (order 0) is constant, so omit that weight - return (weights_ * c.block(1, 0, N_ - 1, 1))(0); - } - }; + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); }; // Chebyshev1Basis diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index a9d778ffda..fb1b052d46 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -30,8 +30,7 @@ namespace gtsam { /** * A matrix abstraction of MxN values at the Basis points. * This class serves as a wrapper over an Eigen matrix. - * @tparam T: the type you would like to evaluate using polynomial - * interpolation. + * @tparam M: The dimension of the type you wish to evaluate. * @param N: the number of Basis points (e.g. Chebyshev points of the second * kind). */ diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index 9f5863567c..31bf4e5ac8 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -140,22 +140,29 @@ TEST(Chebyshev, Decomposition) { //****************************************************************************** TEST(Chebyshev1, Derivative) { - using D = Chebyshev1Basis::Derivative; - Vector c(N); - c << 12, 3, 1; + c << 12, 3, 2; + + Weights D; - // The derivative should be D(12 + 3*x + 2*x*x, x) = 3 + 4*x double x = -1.0; - EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + x = -0.5; - EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + x = 0.3; - EXPECT_DOUBLES_EQUAL(3 + 4 * x, D(N, x)(c), 1e-9); + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); } //****************************************************************************** -Vector3 f(1, 0, 1); +Vector3 f(-6, 1, 0.5); double proxy1(double x, size_t N) { return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); @@ -163,12 +170,62 @@ double proxy1(double x, size_t N) { TEST(Chebyshev1, Derivative2) { const double x = 0.5; - Chebyshev1Basis::Derivative dTdx(N, x); - double analytic_dTdx = f(1) + 4 * f(2) * x; // not too hard to do + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + Matrix numeric_dTdx = numericalDerivative21(proxy1, x, N); - EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), analytic_dTdx, 1e-9); - EXPECT_DOUBLES_EQUAL(analytic_dTdx, dTdx(f), 1e-9); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); } //****************************************************************************** From a0bfaaf12059bce5057aa6feef998e7c91e8f038 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jul 2021 11:59:03 -0400 Subject: [PATCH 15/36] add evaluation factors --- gtsam/basis/basis.i | 64 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 63 insertions(+), 1 deletion(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 2751529c0e..f9f77784ce 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -58,5 +58,67 @@ class ParameterMatrix { Matrix matrix() const; - void print(const string& s="") const; + void print(const string& s = "") const; +}; + +#include + +template +class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); }; From 087a10d322da2b3b1ad6709b324493cddfccf109 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 30 Jul 2021 09:13:26 -0400 Subject: [PATCH 16/36] Wrapper for basis functions (#832) --- gtsam/basis/FitBasis.h | 5 +++- gtsam/basis/basis.i | 41 +++++++++++++++++++++++++--- gtsam/basis/tests/CMakeLists.txt | 2 +- python/CMakeLists.txt | 1 + python/gtsam/preamble/basis.h | 2 +- python/gtsam/specializations/basis.h | 2 +- 6 files changed, 45 insertions(+), 8 deletions(-) diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 5ee14ae6ad..2885e46b4b 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -38,7 +38,10 @@ using Sequence = std::map; using Sample = std::pair; /** - * Class that does Fourier Decomposition via least squares + * Class that does regression via least squares + * Example usage: + * auto fit = FitBasis(3, data_points, noise_model); + * Vector coefficients = fit.parameters(); */ template class FitBasis { diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index f9f77784ce..136fc021d1 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -2,6 +2,10 @@ // basis //************************************************************************* +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. `EvaluationFunctor` + #include class FourierBasis { @@ -64,7 +68,7 @@ class ParameterMatrix { #include template -class EvaluationFactor : gtsam::NoiseModelFactor { +virtual class EvaluationFactor : gtsam::NoiseModelFactor { EvaluationFactor(); EvaluationFactor(gtsam::Key key, const double z, const gtsam::noiseModel::Base* model, const size_t N, @@ -75,7 +79,7 @@ class EvaluationFactor : gtsam::NoiseModelFactor { }; template -class VectorEvaluationFactor : gtsam::NoiseModelFactor { +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { VectorEvaluationFactor(); VectorEvaluationFactor(gtsam::Key key, const Vector& z, const gtsam::noiseModel::Base* model, const size_t N, @@ -95,7 +99,7 @@ typedef gtsam::VectorEvaluationFactor VectorEvaluationFactorChebyshev2D12; template -class VectorComponentFactor : gtsam::NoiseModelFactor { +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { VectorComponentFactor(); VectorComponentFactor(gtsam::Key key, const double z, const gtsam::noiseModel::Base* model, const size_t N, @@ -113,7 +117,7 @@ typedef gtsam::VectorComponentFactor VectorComponentFactorChebyshev2D12; template -class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { ManifoldEvaluationFactor(); ManifoldEvaluationFactor(gtsam::Key key, const T& z, const gtsam::noiseModel::Base* model, const size_t N, @@ -122,3 +126,32 @@ class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { const gtsam::noiseModel::Base* model, const size_t N, double x, double a, double b); }; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +// We'll allow transparent binding of python dict to Sequence in this +// compilation unit using pybind11/stl.h. +// Another alternative would be making Sequence opaque in +// python/gtsam/{preamble, specializations}, but std::map is +// common enough that it may cause collisions, and we don't need +// reference-access anyway. +#include + +template +class FitBasis { + FitBasis(size_t N, const gtsam::Sequence& sequence, + const gtsam::noiseModel::Base* model); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, + size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, + size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt index 8e99ef5ba4..63cad0be6c 100644 --- a/gtsam/basis/tests/CMakeLists.txt +++ b/gtsam/basis/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(base "test*.cpp" "" "gtsam") +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7b8347da5c..b81f3e6f58 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h index a34e730580..d07a75f6fb 100644 --- a/python/gtsam/preamble/basis.h +++ b/python/gtsam/preamble/basis.h @@ -9,4 +9,4 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h index d46ccdc668..da8842eaf4 100644 --- a/python/gtsam/specializations/basis.h +++ b/python/gtsam/specializations/basis.h @@ -9,4 +9,4 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ From dfaa629adeab686b16502c654406cf76b894c60a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 30 Jul 2021 10:21:15 -0700 Subject: [PATCH 17/36] small wrapper updates --- gtsam/basis/basis.i | 12 ------------ python/gtsam/specializations/basis.h | 2 ++ 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 136fc021d1..a71448cc47 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -21,11 +21,6 @@ class FourierBasis { class Chebyshev1Basis { static Matrix CalculateWeights(size_t N, double x); static Matrix WeightMatrix(size_t N, Vector X); - - // TODO need support for nested classes - // class Derivative { - // Derivative(size_t N, double x); - // }; }; class Chebyshev2Basis { @@ -131,13 +126,6 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { // `ComponentDerivativeFactor` #include -// We'll allow transparent binding of python dict to Sequence in this -// compilation unit using pybind11/stl.h. -// Another alternative would be making Sequence opaque in -// python/gtsam/{preamble, specializations}, but std::map is -// common enough that it may cause collisions, and we don't need -// reference-access anyway. -#include template diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h index da8842eaf4..a288ba2483 100644 --- a/python/gtsam/specializations/basis.h +++ b/python/gtsam/specializations/basis.h @@ -10,3 +10,5 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ + +py::bind_map(m_, "Sequence"); From d1b4bfe5677a01ec46df26fc4e9d635d6a2c0d92 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 30 Jul 2021 14:53:16 -0700 Subject: [PATCH 18/36] make EIGEN_UNSUPPORTED OFF by default --- cmake/HandleEigen.cmake | 2 +- gtsam/basis/Basis.h | 25 ++++++++++++++++++++++--- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index eeecf52b98..b21d16885f 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -6,7 +6,7 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us if(NOT GTSAM_USE_SYSTEM_EIGEN) # This option only makes sense if using the embedded copy of Eigen, it is # used to decide whether to *install* the "unsupported" module: - option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" ON) + option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) endif() # Switch for using system Eigen or GTSAM-bundled Eigen diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 8231b44824..c439cb23ee 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -23,7 +23,6 @@ #include #include -#include /** * This file supports creating continuous functions `f(x;p)` as a linear @@ -56,6 +55,26 @@ namespace gtsam { using Weights = Eigen::Matrix; /* 1xN vector */ +/** + * @brief Function for computing the kronecker product of Weight matrix with Identity matrix efficiently. + * The primary reason for this is so that we don't need to use the Eigen Unsupported library. + * + * @tparam M + * @param x + * @return Matrix + */ +template +Matrix kroneckerProductIdentity(const Weights& x) { + const auto I = Eigen::Matrix::Identity(); + Matrix result(M, x.cols() * M); + result.setZero(); + + for (int i = 0; i < x.cols(); i++) { + result.block(0, i*M, M, M) = x(i) * I; + } + return result; +} + /// CRTP Base class for function bases template class GTSAM_EXPORT Basis { @@ -140,7 +159,7 @@ class GTSAM_EXPORT Basis { void calculateJacobian() { using MatrixM = Eigen::Matrix; - H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); + H_ = kroneckerProductIdentity(this->weights_); } public: @@ -323,7 +342,7 @@ class GTSAM_EXPORT Basis { void calculateJacobian() { using MatrixM = Eigen::Matrix; - H_ = Eigen::kroneckerProduct(this->weights_, MatrixM::Identity()); + H_ = kroneckerProductIdentity(this->weights_); } public: From f4e7642d3a8d43d0b46ba322d067e5376d278606 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 31 Jul 2021 20:17:11 -0700 Subject: [PATCH 19/36] cleaner way of getting/setting rows and cols --- gtsam/basis/ParameterMatrix.h | 22 ++++++++++------------ gtsam/basis/tests/testChebyshev2.cpp | 2 +- gtsam/basis/tests/testParameterMatrix.cpp | 8 ++++---- 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index fb1b052d46..09d3b9522d 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -79,29 +79,27 @@ class ParameterMatrix { } /** - * Get the matrix column specified by `index`. - * @param index: The column index to retrieve. + * Set the matrix row specified by `index`. + * @param index: The row index to set. */ - Eigen::Matrix col(size_t index) const { - return matrix_.col(index); + auto row(size_t index) { + return matrix_.row(index); } /** - * Set the matrix row specified by `index`. - * @param index: The row index to set. - * @param row: The Eigen (row) vector used to set the values. + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. */ - void setRow(size_t index, const Eigen::Matrix& row) { - matrix_.row(index) = row; + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); } /** * Set the matrix column specified by `index`. * @param index: The column index to set. - * @param row: The Eigen (column) vector used to set the values. */ - void setCol(size_t index, const Eigen::Matrix& col) { - matrix_.col(index) = col; + auto col(size_t index) { + return matrix_.col(index); } /** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index afeccce87c..4d52f0e788 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -103,7 +103,7 @@ TEST(Chebyshev2, InterpolateVector) { const size_t N = 3; // Create 2x3 matrix with Vectors at Chebyshev points ParameterMatrix<2> X(N); - X.setRow(0, Chebyshev2::Points(N, a, b)); // slope 1 ramp + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp // Check value Vector expected(2); diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp index 0d41248bc5..ec62e8eeab 100644 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -78,13 +78,13 @@ TEST(ParameterMatrix, Setters) { ParameterMatrix params(Matrix::Zero(M, N)); Matrix expected = Matrix::Zero(M, N); - // setRow - params.setRow(0, Vector::Ones(N)); + // row + params.row(0) = Vector::Ones(N); expected.row(0) = Vector::Ones(N); EXPECT(assert_equal(expected, params.matrix())); - // setCol - params.setCol(2, Vector::Ones(M)); + // col + params.col(2) = Vector::Ones(M); expected.col(2) = Vector::Ones(M); EXPECT(assert_equal(expected, params.matrix())); From dcb362716dbf181b95cb448ffb15effe81969aed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 31 Jul 2021 20:34:47 -0700 Subject: [PATCH 20/36] C++11 return types --- gtsam/basis/ParameterMatrix.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index 09d3b9522d..df2d9f62e9 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -82,7 +82,7 @@ class ParameterMatrix { * Set the matrix row specified by `index`. * @param index: The row index to set. */ - auto row(size_t index) { + auto row(size_t index) -> Eigen::Block { return matrix_.row(index); } @@ -98,7 +98,7 @@ class ParameterMatrix { * Set the matrix column specified by `index`. * @param index: The column index to set. */ - auto col(size_t index) { + auto col(size_t index) -> Eigen::Block { return matrix_.col(index); } From 63a9561529f34ccaa8415e943560653b460e1fb6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Aug 2021 15:56:48 -0400 Subject: [PATCH 21/36] minor improvement --- gtsam/basis/Basis.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index c439cb23ee..d6c4e74afe 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -56,21 +56,21 @@ namespace gtsam { using Weights = Eigen::Matrix; /* 1xN vector */ /** - * @brief Function for computing the kronecker product of Weight matrix with Identity matrix efficiently. - * The primary reason for this is so that we don't need to use the Eigen Unsupported library. - * - * @tparam M - * @param x - * @return Matrix + * @brief Function for computing the kronecker product of Weight matrix with + * Identity matrix efficiently. The primary reason for this is so that we don't + * need to use the Eigen Unsupported library. + * + * @tparam M Size of the identity matrix. + * @param x The weights of the polynomial. + * @return Matrix */ -template +template Matrix kroneckerProductIdentity(const Weights& x) { - const auto I = Eigen::Matrix::Identity(); Matrix result(M, x.cols() * M); result.setZero(); for (int i = 0; i < x.cols(); i++) { - result.block(0, i*M, M, M) = x(i) * I; + result.block(0, i * M, M, M).diagonal().array() = x(i); } return result; } From 3ebe2211e6cfc1701c3422e18b89e6c5d0ea02db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Aug 2021 16:30:13 -0400 Subject: [PATCH 22/36] rework base binding so we can import pybind11/stl.h --- gtsam/base/base.i | 59 ++++++------------------------------ python/gtsam/gtsam.tpl | 1 + python/gtsam/preamble/base.h | 3 +- 3 files changed, 12 insertions(+), 51 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index c24b04088a..1a95ae0566 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -33,61 +33,20 @@ class IndexPair { size_t j() const; }; -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; - -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists +template +class DSFMap { + DSFMap(); + KEY find(const KEY& key) const; + void merge(const KEY& x, const KEY& y); + std::map sets(); }; -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; +class IndexPairVector; +class IndexPairSetMap; +class IndexPairSet; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; - -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; - #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 93e7ffbafd..79d56fff11 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -12,6 +12,7 @@ #include #include +#include #include #include #include diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index 626b47ae4a..0699ba6f2f 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -11,6 +11,7 @@ * mutations on Python side will not be reflected on C++. */ -PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSetMap); +PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector); PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector From 2a9106dcc8db2697c1141c465296838aac1780be Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Aug 2021 16:30:27 -0400 Subject: [PATCH 23/36] remove custom binding for sequence --- python/gtsam/specializations/basis.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h index a288ba2483..da8842eaf4 100644 --- a/python/gtsam/specializations/basis.h +++ b/python/gtsam/specializations/basis.h @@ -10,5 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_map(m_, "Sequence"); From efcae9be9f58f4ef3a629a7d7c3089b958ab1f51 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Aug 2021 11:25:18 -0400 Subject: [PATCH 24/36] comment out FitBasis until we get it working --- gtsam/basis/basis.i | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index a71448cc47..4980c40aa3 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -125,21 +125,21 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { // TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and // `ComponentDerivativeFactor` -#include - -template -class FitBasis { - FitBasis(size_t N, const gtsam::Sequence& sequence, - const gtsam::noiseModel::Base* model); - - static gtsam::NonlinearFactorGraph NonlinearGraph( - const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, - size_t N); - static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( - const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, - size_t N); - Parameters parameters() const; -}; +// TODO(Varun): Get working +// #include +// template +// class FitBasis { +// FitBasis(size_t N, const gtsam::Sequence& sequence, +// const gtsam::noiseModel::Base* model); + +// static gtsam::NonlinearFactorGraph NonlinearGraph( +// const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, +// size_t N); +// static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( +// const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, +// size_t N); +// Parameters parameters() const; +// }; } // namespace gtsam From 98a9265442f78b16dd843db1e43fa33f5c328430 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Aug 2021 11:25:21 -0400 Subject: [PATCH 25/36] Revert "rework base binding so we can import pybind11/stl.h" This reverts commit 3ebe2211e6cfc1701c3422e18b89e6c5d0ea02db. --- gtsam/base/base.i | 59 ++++++++++++++++++++++++++++++------ python/gtsam/gtsam.tpl | 1 - python/gtsam/preamble/base.h | 3 +- 3 files changed, 51 insertions(+), 12 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 1a95ae0566..c24b04088a 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -33,20 +33,61 @@ class IndexPair { size_t j() const; }; -template -class DSFMap { - DSFMap(); - KEY find(const KEY& key) const; - void merge(const KEY& x, const KEY& y); - std::map sets(); +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists }; -class IndexPairVector; -class IndexPairSetMap; -class IndexPairSet; +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; + #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 79d56fff11..93e7ffbafd 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -12,7 +12,6 @@ #include #include -#include #include #include #include diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index 0699ba6f2f..626b47ae4a 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -11,7 +11,6 @@ * mutations on Python side will not be reflected on C++. */ -PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSetMap); -PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector); +PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector From d074b6173fc0eb6c805ca8af846b10f6f57fa71a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Aug 2021 17:21:26 -0400 Subject: [PATCH 26/36] address review comments --- gtsam/basis/Basis.h | 195 +++++++++++++++++++++++++------------ gtsam/basis/BasisFactors.h | 7 +- 2 files changed, 136 insertions(+), 66 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index d6c4e74afe..13845f3e1f 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -12,7 +12,7 @@ /** * @file Basis.h * @brief Compute an interpolating basis - * @author Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Jing Dong, Frank Dellaert * @date July 4, 2020 */ @@ -28,10 +28,11 @@ * This file supports creating continuous functions `f(x;p)` as a linear * combination of `basis functions` such as the Fourier basis on SO(2) or a set * of Chebyshev polynomials on [-1,1]. + * * In the expression `f(x;p)` the variable `x` is * the continuous argument at which the function is evaluated, and `p` are - * parameter which are coefficients of the different basis functions, - * e.g. p = (4, 3, 2) => 4 + 3x + 2x^2 for a polynomial. + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. * However, different parameterizations are also possible. * The `Basis` class below defines a number of functors that can be used to @@ -49,6 +50,9 @@ - type `Parameters`: the parameters `p` in f(x;p) - `CalculateWeights(size_t N, double x, double a=default, double b=default)` - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the interpolation + coefficients for the basis functions to interpolate at the specified point `x`. */ namespace gtsam { @@ -56,21 +60,22 @@ namespace gtsam { using Weights = Eigen::Matrix; /* 1xN vector */ /** - * @brief Function for computing the kronecker product of Weight matrix with - * Identity matrix efficiently. The primary reason for this is so that we don't - * need to use the Eigen Unsupported library. + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. * * @tparam M Size of the identity matrix. - * @param x The weights of the polynomial. - * @return Matrix + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] */ template -Matrix kroneckerProductIdentity(const Weights& x) { - Matrix result(M, x.cols() * M); +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); result.setZero(); - for (int i = 0; i < x.cols(); i++) { - result.block(0, i * M, M, M).diagonal().array() = x(i); + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); } return result; } @@ -80,9 +85,9 @@ template class GTSAM_EXPORT Basis { public: /** - * Call weights for all x in vector X. + * Calculate weights for all x in vector X. * Returns M*N matrix where M is the size of the vector X, - * and N is the number of basis function. + * and N is the number of basis functions. */ static Matrix WeightMatrix(size_t N, const Vector& X) { Matrix W(X.size(), N); @@ -92,9 +97,13 @@ class GTSAM_EXPORT Basis { } /** - * Call weights for all x in vector X, with interval [a,b]. - * Returns M*N matrix where M is the size of the vector X, - * and N is the number of basis function. + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. */ static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { Matrix W(X.size(), N); @@ -104,11 +113,11 @@ class GTSAM_EXPORT Basis { } /** - * EvaluationFunctor calculate f(x;p) at a given `x`, applied to Parameters - * `p`. This functor is used to evaluate a parameterized function at a given - * scalar value x. When given a specific N*1 vector of Parameters, returns the - * scalar value of the function at x, possibly with Jacobian wrpt the - * parameters. + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. */ class EvaluationFunctor { protected: @@ -127,16 +136,16 @@ class GTSAM_EXPORT Basis { : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} /// Regular 1D evaluation - double apply(const typename DERIVED::Parameters& f, + double apply(const typename DERIVED::Parameters& p, OptionalJacobian<-1, -1> H = boost::none) const { if (H) *H = weights_; - return (weights_ * f)(0); + return (weights_ * p)(0); } /// c++ sugar - double operator()(const typename DERIVED::Parameters& f, + double operator()(const typename DERIVED::Parameters& p, OptionalJacobian<-1, -1> H = boost::none) const { - return apply(f, H); // might call apply in derived + return apply(p, H); // might call apply in derived } void print(const std::string& s = "") const { @@ -148,7 +157,7 @@ class GTSAM_EXPORT Basis { * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M - * corresponding functions at x, possibly with Jacobian wrpt the parameters. + * corresponding functions at x, possibly with Jacobians wrpt the parameters. */ template class VectorEvaluationFunctor : protected EvaluationFunctor { @@ -157,8 +166,15 @@ class GTSAM_EXPORT Basis { using Jacobian = Eigen::Matrix; Jacobian H_; + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ void calculateJacobian() { - using MatrixM = Eigen::Matrix; H_ = kroneckerProductIdentity(this->weights_); } @@ -180,22 +196,25 @@ class GTSAM_EXPORT Basis { } /// M-dimensional evaluation - VectorM apply(const ParameterMatrix& F, + VectorM apply(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F.matrix() * this->weights_.transpose(); + return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& F, + VectorM operator()(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { - return apply(F, H); + return apply(P, H); } }; /** - * Given M*N Matrix of M-vectors at N Chebyshev points, predict component of - * row vector at given i, with 0 class VectorComponentFunctor : public EvaluationFunctor { @@ -203,6 +222,16 @@ class GTSAM_EXPORT Basis { using Jacobian = Eigen::Matrix; size_t rowIndex_; Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ void calculateJacobian(size_t N) { H_.setZero(1, M * N); for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) @@ -225,17 +254,17 @@ class GTSAM_EXPORT Basis { calculateJacobian(N); } - /// Calculate component of component rowIndex_ of F - double apply(const ParameterMatrix& F, + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& F, + double operator()(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { - return apply(F, H); + return apply(P, H); } }; @@ -243,9 +272,15 @@ class GTSAM_EXPORT Basis { * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. * This functor is used to evaluate a parameterized function at a given scalar * value x. When given a specific M*N parameters, returns an M-vector the M - * corresponding functions at x, possibly with Jacobian wrpt the parameters. + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. */ - /// Manifold interpolation template class ManifoldEvaluationFunctor : public VectorEvaluationFunctor::dimension> { @@ -264,10 +299,10 @@ class GTSAM_EXPORT Basis { : Base(N, x, a, b) {} /// Manifold evaluation - T apply(const ParameterMatrix& F, + T apply(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { // Interpolate the M-dimensional vector to yield a vector in tangent space - Eigen::Matrix xi = Base::operator()(F, H); + Eigen::Matrix xi = Base::operator()(P, H); // Now call retract with this M-vector, possibly with derivatives Eigen::Matrix D_result_xi; @@ -283,9 +318,9 @@ class GTSAM_EXPORT Basis { } /// c++ sugar - T operator()(const ParameterMatrix& F, + T operator()(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { - return apply(F, H); // might call apply in derived + return apply(P, H); // might call apply in derived } }; @@ -309,7 +344,13 @@ class GTSAM_EXPORT Basis { } }; - /// Given values f at the Chebyshev points, predict derivative at x + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ class DerivativeFunctor : protected DerivativeFunctorBase { public: /// For serialization @@ -320,19 +361,26 @@ class GTSAM_EXPORT Basis { DerivativeFunctor(size_t N, double x, double a, double b) : DerivativeFunctorBase(N, x, a, b) {} - double apply(const typename DERIVED::Parameters& f, + double apply(const typename DERIVED::Parameters& p, OptionalJacobian H = boost::none) const { if (H) *H = this->weights_; - return (this->weights_ * f)(0); + return (this->weights_ * p)(0); } /// c++ sugar - double operator()(const typename DERIVED::Parameters& f, + double operator()(const typename DERIVED::Parameters& p, OptionalJacobian H = boost::none) const { - return apply(f, H); // might call apply in derived + return apply(p, H); // might call apply in derived } }; - /// Compute derivative vector of ParameterMatrix at specified point. + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ template class VectorDerivativeFunctor : protected DerivativeFunctorBase { protected: @@ -340,8 +388,15 @@ class GTSAM_EXPORT Basis { using Jacobian = Eigen::Matrix; Jacobian H_; + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ void calculateJacobian() { - using MatrixM = Eigen::Matrix; H_ = kroneckerProductIdentity(this->weights_); } @@ -362,22 +417,25 @@ class GTSAM_EXPORT Basis { calculateJacobian(); } - VectorM apply(const ParameterMatrix& F, + VectorM apply(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F.matrix() * this->weights_.transpose(); + return P.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()( - const ParameterMatrix& F, + const ParameterMatrix& P, OptionalJacobian H = boost::none) const { - return apply(F, H); + return apply(P, H); } }; /** - * Given M*N Matrix of M-vectors at N Chebyshev points, predict derivative for - * given row i, with 0<=i class ComponentDerivativeFunctor : protected DerivativeFunctorBase { @@ -386,6 +444,15 @@ class GTSAM_EXPORT Basis { size_t rowIndex_; Jacobian H_; + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ void calculateJacobian(size_t N) { H_.setZero(1, M * N); for (int j = 0; j < this->weights_.size(); j++) @@ -408,22 +475,22 @@ class GTSAM_EXPORT Basis { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F - double apply(const ParameterMatrix& F, + double apply(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { if (H) *H = H_; - return F.row(rowIndex_) * this->weights_.transpose(); + return P.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& F, + double operator()(const ParameterMatrix& P, OptionalJacobian H = boost::none) const { - return apply(F, H); + return apply(P, H); } }; // Vector version for MATLAB :-( - static double Derivative(double x, const Vector& f, // + static double Derivative(double x, const Vector& p, // OptionalJacobian H = boost::none) { - return DerivativeFunctor(x)(f.transpose(), H); + return DerivativeFunctor(x)(p.transpose(), H); } }; diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index aed9447330..1db1f9e759 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -11,7 +11,9 @@ /** * @file BasisFactors.h + * @brief Factor definitions for various Basis functors. * @author Varun Agrawal + * @date July 4, 2020 **/ #pragma once @@ -22,8 +24,9 @@ namespace gtsam { /** - * Factor for scalar BASIS evaluation. - * @param BASIS: The basis class to use e.g. Chebyshev2 + * @brief Factor for scalar BASIS evaluation. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 */ template class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { From f8b3a59a04878466828fc0ffae77a99c5ec12143 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Aug 2021 19:01:23 -0400 Subject: [PATCH 27/36] add CRTP methods for Fourier --- gtsam/basis/Fourier.h | 49 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 46 insertions(+), 3 deletions(-) diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 2436ffe44b..32e1c98fee 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -30,8 +30,12 @@ class GTSAM_EXPORT FourierBasis : public Basis { using DiffMatrix = Eigen::Matrix; /** - * Evaluate Real Fourier Weights of size N - * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights */ static Weights CalculateWeights(size_t N, double x) { Weights b(N); @@ -43,6 +47,25 @@ class GTSAM_EXPORT FourierBasis : public Basis { return b; } + /** + * + */ + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + /** * Compute D = differentiation matrix. * Given coefficients c of a Fourier series c, D*c are the values of c'. @@ -59,11 +82,31 @@ class GTSAM_EXPORT FourierBasis : public Basis { return D; } - /// Get weights at a given x that calculate the derivative. + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ static Weights DerivativeWeights(size_t N, double x) { return CalculateWeights(N, x) * DifferentiationMatrix(N); } + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + }; // FourierBasis } // namespace gtsam From 037b7644c931caef5cf88005a8d624412060e07e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Aug 2021 19:08:17 -0400 Subject: [PATCH 28/36] formatting and document example for Fourier --- gtsam/basis/Basis.h | 14 ++++++++++++-- gtsam/basis/Fourier.h | 12 ++++-------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 13845f3e1f..d8bd28c1a0 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -51,8 +51,18 @@ - `CalculateWeights(size_t N, double x, double a=default, double b=default)` - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` - where `Weights` is an N*1 row vector which defines the interpolation - coefficients for the basis functions to interpolate at the specified point `x`. + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. */ namespace gtsam { diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 32e1c98fee..6943150d72 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -32,10 +32,10 @@ class GTSAM_EXPORT FourierBasis : public Basis { /** * @brief Evaluate Real Fourier Weights of size N in interval [a, b], * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) - * + * * @param N The degree of the polynomial to use. * @param x The point at which to compute the derivaive weights. - * @return Weights + * @return Weights */ static Weights CalculateWeights(size_t N, double x) { Weights b(N); @@ -47,19 +47,15 @@ class GTSAM_EXPORT FourierBasis : public Basis { return b; } - /** - * - */ - /** * @brief Evaluate Real Fourier Weights of size N in interval [a, b], * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) - * + * * @param N The degree of the polynomial to use. * @param x The point at which to compute the weights. * @param a Lower bound of interval. * @param b Upper bound of interval. - * @return Weights + * @return Weights */ static Weights CalculateWeights(size_t N, double x, double a, double b) { // TODO(Varun) How do we enforce an interval for Fourier series? From e70988e0cf2f0c2a7413d89e90782bf835483df2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 Aug 2021 11:15:15 -0400 Subject: [PATCH 29/36] use container instead of typedef in map definition for FitBasis --- gtsam/basis/basis.i | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 4980c40aa3..a64ae52390 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -4,7 +4,8 @@ namespace gtsam { -// TODO(gerry): add all the Functors to the Basis interfaces, e.g. `EvaluationFunctor` +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` #include @@ -125,21 +126,20 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { // TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and // `ComponentDerivativeFactor` -// TODO(Varun): Get working -// #include -// template -// class FitBasis { -// FitBasis(size_t N, const gtsam::Sequence& sequence, -// const gtsam::noiseModel::Base* model); - -// static gtsam::NonlinearFactorGraph NonlinearGraph( -// const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, -// size_t N); -// static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( -// const gtsam::Sequence& sequence, const gtsam::noiseModel::Base* model, -// size_t N); -// Parameters parameters() const; -// }; +#include +template +class FitBasis { + FitBasis(size_t N, const std::map& sequence, + const gtsam::noiseModel::Base* model); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; } // namespace gtsam From 7cf510f449f6734657596515106cde14d6581288 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 02:41:20 -0400 Subject: [PATCH 30/36] swap around Chebyshev1 and Chebyshev2 + address other comments --- gtsam/basis/Chebyshev.cpp | 55 +++++++++++++++++++------------------ gtsam/basis/Chebyshev.h | 58 +++++++++++++++++++-------------------- 2 files changed, 56 insertions(+), 57 deletions(-) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp index 76aa0eb5f6..3b5825fc33 100644 --- a/gtsam/basis/Chebyshev.cpp +++ b/gtsam/basis/Chebyshev.cpp @@ -12,7 +12,7 @@ /** * @file Chebyshev.cpp * @brief Chebyshev basis decompositions - * @author Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Jing Dong, Frank Dellaert * @date July 4, 2020 */ @@ -32,10 +32,35 @@ namespace gtsam { * @param t2 New upper limit. * @return double */ -double scale(double x, double a, double b, double t1, double t2) { +static double scale(double x, double a, double b, double t1, double t2) { return ((t2 - t1) * (x - a) / (b - a)) + t1; } +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, double b) { Weights Ux(N); @@ -50,6 +75,7 @@ Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, } return Ux; } + Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, double b) { Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); @@ -69,29 +95,4 @@ Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, return weights; } -Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, - double b) { - Weights Tx(1, N); - - x = scale(x, a, b, -1, 1); - - Tx(0) = 1; - Tx(1) = x; - for (size_t i = 2; i < N; i++) { - // instead of cos(i*acos(x)), this recurrence is much faster - Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); - } - return Tx; -} - -Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, - double b) { - Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); - Weights weights = Weights::Zero(N); - for (size_t n = 1; n < N; n++) { - weights(n) = n * Ux(n - 1); - } - return weights; -} - } // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 5084e633f9..d16ccfaac0 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -12,7 +12,7 @@ /** * @file Chebyshev.h * @brief Chebyshev basis decompositions - * @author Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Jing Dong, Frank Dellaert * @date July 4, 2020 */ @@ -26,20 +26,18 @@ namespace gtsam { /** - * Basis of Chebyshev polynomials of the second kind. - * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind - * These are typically denoted with the symbol U_n, where n is the degree. + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. * The parameter N is the number of coefficients, i.e., N = n+1. - * In contrast to the templates in Chebyshev2, the classes below specify - * basis functions, weighted combinations of which are used to approximate - * functions. In this sense, they are like the sines and cosines of the Fourier - * basis. */ -struct Chebyshev2Basis : Basis { +struct Chebyshev1Basis : Basis { using Parameters = Eigen::Matrix; + Parameters parameters_; + /** - * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) * * @param N Degree of the polynomial. * @param x Point to evaluate polynomial at. @@ -51,6 +49,15 @@ struct Chebyshev2Basis : Basis { /** * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. * * @param N Degree of the polynomial. * @param x Point to evaluate polynomial at. @@ -60,22 +67,23 @@ struct Chebyshev2Basis : Basis { */ static Weights DerivativeWeights(size_t N, double x, double a = -1, double b = 1); -}; -// Chebyshev2Basis +}; // Chebyshev1Basis /** - * Basis of Chebyshev polynomials of the first kind - * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind - * These are typically denoted with the symbol T_n, where n is the degree. + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. */ -struct Chebyshev1Basis : Basis { +struct Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; - Parameters parameters_; - /** - * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). * * @param N Degree of the polynomial. * @param x Point to evaluate polynomial at. @@ -87,15 +95,6 @@ struct Chebyshev1Basis : Basis { /** * @brief Evaluate Chebyshev derivative at x. - * The derivative weights are pre-multiplied to the polynomial Parameters. - * - * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) - * I.e. the derivative fo a first kind cheb is just a second kind cheb - * So, we define a second kind basis here of order N-1 - * Note that it has one less weight. - * - * The Parameters pertain to 1st kind chebs up to order N-1 - * But of course the first one (order 0) is constant, so omit that weight. * * @param N Degree of the polynomial. * @param x Point to evaluate polynomial at. @@ -105,7 +104,6 @@ struct Chebyshev1Basis : Basis { */ static Weights DerivativeWeights(size_t N, double x, double a = -1, double b = 1); -}; -// Chebyshev1Basis +}; // Chebyshev2Basis } // namespace gtsam From 2d3dd25de97cacf318fd46a15d6829773695cf62 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 03:21:37 -0400 Subject: [PATCH 31/36] address review comments --- gtsam/basis/BasisFactors.h | 202 +++++++++++++++++++++++++++++++++++-- gtsam/basis/Chebyshev2.cpp | 4 +- gtsam/basis/Chebyshev2.h | 23 +++-- gtsam/basis/FitBasis.h | 12 ++- gtsam/basis/basis.i | 2 +- 5 files changed, 222 insertions(+), 21 deletions(-) diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 1db1f9e759..f23443a9e1 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -24,7 +24,13 @@ namespace gtsam { /** - * @brief Factor for scalar BASIS evaluation. + * @brief Factor for enforcing the scalar value of a function is the same as + * that of polynomial BASIS representation when using a pseudo-spectral + * parameterization. + * + * If we have a scalar-valued function which gives us a value `z`, this factor + * enforces the polynomial basis, when evaluated at the same point, gives us the + * same value `z`. * * @tparam BASIS The basis class to use e.g. Chebyshev2 */ @@ -68,7 +74,14 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { }; /** - * Prior factor for BASIS vector evaluation on ParameterMatrix of size (M,N). + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued function at the same point, when + * using a pseudo-spectral parameterization. + * + * If we have a vector-valued function which gives us a value `z`, this factor + * enforces the polynomial basis used to fit the function, when evaluated at the + * same point, gives us the same value `z`. + * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector. */ @@ -81,12 +94,34 @@ class GTSAM_EXPORT VectorEvaluationFactor public: VectorEvaluationFactor() {} + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The vector-value of the function to estimate at the point `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::template VectorEvaluationFunctor(N, x)) {} + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The vector-value of the function to estimate at the point `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) @@ -97,8 +132,14 @@ class GTSAM_EXPORT VectorEvaluationFactor }; /** - * Create a measurement on any scalar entry (chosen in the constructor) of a - * P-dimensional vector computed by a basis parameterization. + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to a vector-valued function at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param P: Size of the fixed-size vector. * @@ -116,11 +157,39 @@ class GTSAM_EXPORT VectorComponentFactor public: VectorComponentFactor() {} + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the vector-valued + * function to estimate at the point `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ 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

(N, i, x)) {} + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the vector-valued + * function to estimate at the point `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b) : Base( @@ -132,9 +201,21 @@ class GTSAM_EXPORT VectorComponentFactor }; /** - * Prior factor for BASIS Manifold evaluation on type T. + * For a function which evaluates to a type T, this unary factor enforces that + * the polynomial basis, when evaluated at the same point, gives us the same + * value of T. + * + * This is done via computations on the tangent space of the + * manifold of T. + * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param T: Object type which is synthesized by the functor. + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. */ template class GTSAM_EXPORT ManifoldEvaluationFactor @@ -145,11 +226,33 @@ class GTSAM_EXPORT ManifoldEvaluationFactor public: ManifoldEvaluationFactor() {} + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The value of the function to estimate at the point `x`. + * @param model + * @param N The degree of the polynomial. + * @param x The point at which the function is evaluated. + */ ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The value of the function to estimate at the point `x`. + * @param model + * @param N The degree of the polynomial. + * @param x The point at which the function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) : Base( @@ -161,7 +264,10 @@ class GTSAM_EXPORT ManifoldEvaluationFactor }; /** - * Factor for scalar BASIS derivative evaluation. + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value of a function at a specified point + * `x`. + * * @param BASIS: The basis class to use e.g. Chebyshev2 */ template @@ -173,10 +279,32 @@ class GTSAM_EXPORT DerivativeFactor public: DerivativeFactor() {} + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The result of the function to estimate evaluated at `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The result of the function to estimate evaluated at `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ 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)) {} @@ -185,7 +313,10 @@ class GTSAM_EXPORT DerivativeFactor }; /** - * Prior factor for BASIS vector derivative on ParameterMatrix of size (M,N). + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the vector value of a function at a specified point + * `x`. + * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector derivative. */ @@ -199,11 +330,33 @@ class GTSAM_EXPORT VectorDerivativeFactor public: VectorDerivativeFactor() {} + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The vector result of the function to estimate evaluated at `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, Func(N, x)) {} + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The vector result of the function to estimate evaluated at `x`. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) @@ -213,7 +366,10 @@ class GTSAM_EXPORT VectorDerivativeFactor }; /** - * Prior factor for BASIS component derivative on ParameterMatrix of size (M,N). + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued function at a specified point `x`. + * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param P: Size of the control component derivative. */ @@ -227,11 +383,39 @@ class GTSAM_EXPORT ComponentDerivativeFactor public: ComponentDerivativeFactor() {} + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar at a specific index `i` of the vector value of the + * function to estimate evaluated at `x`. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ 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)) {} + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar at specified index `i` of the vector value of the + * function to estimate evaluated at `x`. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 8e59001d5d..44876b6e91 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -12,7 +12,7 @@ /** * @file Chebyshev2.cpp * @brief Chebyshev parameterizations on Chebyshev points of second kind - * @author Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Jing Dong, Frank Dellaert * @date July 4, 2020 */ @@ -202,4 +202,4 @@ Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { return weights; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index dd2c5320ac..044f349949 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -11,12 +11,23 @@ /** * @file Chebyshev2.h - * @brief Chebyshev parameterizations on Chebyshev points of second kind. - * This is different from Chebyshev.h since it leverage ideas from - * pseudo-spectral optimization, i.e. we don't decompose into basis - * functions, rather estimate function parameters that enforce function - * nodes at Chebyshev points. - * @author Varun Agrawal, Frank Dellaert + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert * @date July 4, 2020 */ diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 2885e46b4b..684f716dc1 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -40,7 +40,7 @@ using Sample = std::pair; /** * Class that does regression via least squares * Example usage: - * auto fit = FitBasis(3, data_points, noise_model); + * auto fit = FitBasis(data_points, noise_model, 3); * Vector coefficients = fit.parameters(); */ template @@ -74,8 +74,14 @@ class FitBasis { return gfg; } - /// Constructor - FitBasis(size_t N, const Sequence& sequence, const SharedNoiseModel& model) { + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); VectorValues solution = gfg->optimize(); parameters_ = solution.at(0); diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index a64ae52390..a7f9db994d 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -63,7 +63,7 @@ class ParameterMatrix { #include -template +template virtual class EvaluationFactor : gtsam::NoiseModelFactor { EvaluationFactor(); EvaluationFactor(gtsam::Key key, const double z, From 9647d0e0a66d42ef0d8016ad9714cffe92c108db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 08:06:30 -0400 Subject: [PATCH 32/36] fix FitBasis wrapper --- gtsam/basis/basis.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index a7f9db994d..f7149fc9d5 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -130,8 +130,8 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { template class FitBasis { - FitBasis(size_t N, const std::map& sequence, - const gtsam::noiseModel::Base* model); + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); static gtsam::NonlinearFactorGraph NonlinearGraph( const std::map& sequence, From f7105b2ff75df94ef8679faaf50c19339baf84c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Aug 2021 09:30:46 -0400 Subject: [PATCH 33/36] fixes --- gtsam/basis/basis.i | 3 ++- gtsam/basis/tests/testChebyshev.cpp | 2 +- gtsam/basis/tests/testChebyshev2.cpp | 2 +- gtsam/basis/tests/testFourier.cpp | 2 +- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index f7149fc9d5..8f06fd2e13 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -63,7 +63,8 @@ class ParameterMatrix { #include -template +template virtual class EvaluationFactor : gtsam::NoiseModelFactor { EvaluationFactor(); EvaluationFactor(gtsam::Key key, const double z, diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index 31bf4e5ac8..64c925886d 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -130,7 +130,7 @@ TEST(Chebyshev, Decomposition) { } // Do Chebyshev Decomposition - FitBasis actual(N, sequence, model); + FitBasis actual(sequence, model, N); // Check Vector expected = Vector::Zero(N); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 4d52f0e788..4cee70daf9 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -131,7 +131,7 @@ TEST(Chebyshev2, Decomposition) { } // Do Chebyshev Decomposition - FitBasis actual(3, sequence, model); + FitBasis actual(sequence, model, 3); // Check Vector expected(3); diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index fa3482ab7d..7a53cfcc92 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -148,7 +148,7 @@ TEST(Basis, Decomposition) { } // Do Fourier Decomposition - FitBasis actual(3, sequence, model); + FitBasis actual(sequence, model, 3); // Check EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); From b446d9df9b9e8d532f3c95288534d3077880e7ea Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 24 Aug 2021 17:19:52 -0400 Subject: [PATCH 34/36] address review comments --- gtsam/basis/BasisFactors.h | 21 ++++++++------------- gtsam/basis/Chebyshev2.h | 4 ++-- gtsam/basis/FitBasis.h | 7 ++++++- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index f23443a9e1..b9f36633f3 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -24,13 +24,9 @@ namespace gtsam { /** - * @brief Factor for enforcing the scalar value of a function is the same as - * that of polynomial BASIS representation when using a pseudo-spectral - * parameterization. - * - * If we have a scalar-valued function which gives us a value `z`, this factor - * enforces the polynomial basis, when evaluated at the same point, gives us the - * same value `z`. + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * represenation is the same as the measured function value `z` when using a + * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2 */ @@ -78,9 +74,9 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * of size (M, N) is equal to a vector-valued function at the same point, when * using a pseudo-spectral parameterization. * - * If we have a vector-valued function which gives us a value `z`, this factor - * enforces the polynomial basis used to fit the function, when evaluated at the - * same point, gives us the same value `z`. + * If we have a vector-valued function which gives us a value `z` at input `x`, + * this factor enforces the polynomial basis used to approximate the function + * gives us the same value `z` at `x`. * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector. @@ -201,9 +197,8 @@ class GTSAM_EXPORT VectorComponentFactor }; /** - * For a function which evaluates to a type T, this unary factor enforces that - * the polynomial basis, when evaluated at the same point, gives us the same - * value of T. + * For a function which gives us values of type T i.e. `T z = f(x)`, this unary factor enforces that + * the polynomial basis, when evaluated at `x`, gives us the same `z`. * * This is done via computations on the tangent space of the * manifold of T. diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 044f349949..28590961d4 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -43,8 +43,8 @@ namespace gtsam { /** * Chebyshev Interpolation on Chebyshev points of the second kind - * Note that N here, the #points, is one less than N from Trefethen00book - * (pg.42) + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. */ class GTSAM_EXPORT Chebyshev2 : public Basis { public: diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 684f716dc1..f5cb99bd7e 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -40,8 +40,13 @@ using Sample = std::pair; /** * Class that does regression via least squares * Example usage: - * auto fit = FitBasis(data_points, noise_model, 3); + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. */ template class FitBasis { From 4558af2ff2f640a8fbdabfa844673187e76a1ca0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 25 Aug 2021 16:25:29 -0400 Subject: [PATCH 35/36] update docs to use measurement instead of function --- gtsam/basis/BasisFactors.h | 72 ++++++++++++++++++++------------------ 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index b9f36633f3..cdbdb68476 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -25,7 +25,7 @@ namespace gtsam { /** * @brief Factor for enforcing the scalar value of the polynomial BASIS - * represenation is the same as the measured function value `z` when using a + * represenation at `x` is the same as the measurement `z` when using a * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2 @@ -42,7 +42,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * @brief Construct a new EvaluationFactor object * * @param key Symbol for value to optimize. - * @param z The result of the functional evaluation. + * @param z The measurement value. * @param model Noise model * @param N The degree of the polynomial. * @param x The point at which to evaluate the polynomial. @@ -55,7 +55,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { * @brief Construct a new EvaluationFactor object * * @param key Symbol for value to optimize. - * @param z The result of the functional evaluation. + * @param z The measurement value. * @param model Noise model * @param N The degree of the polynomial. * @param x The point at which to evaluate the polynomial. @@ -71,12 +71,15 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { /** * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix - * of size (M, N) is equal to a vector-valued function at the same point, when + * of size (M, N) is equal to a vector-valued measurement at the same point, + when * using a pseudo-spectral parameterization. * - * If we have a vector-valued function which gives us a value `z` at input `x`, - * this factor enforces the polynomial basis used to approximate the function - * gives us the same value `z` at `x`. + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector. @@ -95,7 +98,7 @@ class GTSAM_EXPORT VectorEvaluationFactor * * @param key The key to the ParameterMatrix object used to represent the * polynomial. - * @param z The vector-value of the function to estimate at the point `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -111,7 +114,7 @@ class GTSAM_EXPORT VectorEvaluationFactor * * @param key The key to the ParameterMatrix object used to represent the * polynomial. - * @param z The vector-value of the function to estimate at the point `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -129,7 +132,7 @@ class GTSAM_EXPORT VectorEvaluationFactor /** * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix - * of size (P, N) is equal to a vector-valued function at the same point, when + * of size (P, N) is equal to specified measurement at the same point, when * using a pseudo-spectral parameterization. * * This factor is similar to `VectorEvaluationFactor` with the key difference @@ -158,8 +161,8 @@ class GTSAM_EXPORT VectorComponentFactor * * @param key The key to the ParameterMatrix object used to represent the * polynomial. - * @param z The scalar value at a specified index `i` of the vector-valued - * function to estimate at the point `x`. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. * @param model The noise model. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar @@ -176,8 +179,8 @@ class GTSAM_EXPORT VectorComponentFactor * * @param key The key to the ParameterMatrix object used to represent the * polynomial. - * @param z The scalar value at a specified index `i` of the vector-valued - * function to estimate at the point `x`. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. * @param model The noise model. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar @@ -197,8 +200,9 @@ class GTSAM_EXPORT VectorComponentFactor }; /** - * For a function which gives us values of type T i.e. `T z = f(x)`, this unary factor enforces that - * the polynomial basis, when evaluated at `x`, gives us the same `z`. + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. * * This is done via computations on the tangent space of the * manifold of T. @@ -226,10 +230,10 @@ class GTSAM_EXPORT ManifoldEvaluationFactor * * @param key Key for the state matrix parameterizing the function to estimate * via the BASIS. - * @param z The value of the function to estimate at the point `x`. - * @param model + * @param z The measurement value. + * @param model The noise model. * @param N The degree of the polynomial. - * @param x The point at which the function is evaluated. + * @param x The point at which the estimated function is evaluated. */ ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x) @@ -241,10 +245,10 @@ class GTSAM_EXPORT ManifoldEvaluationFactor * * @param key Key for the state matrix parameterizing the function to estimate * via the BASIS. - * @param z The value of the function to estimate at the point `x`. - * @param model + * @param z The measurement value. + * @param model The noise model. * @param N The degree of the polynomial. - * @param x The point at which the function is evaluated. + * @param x The point at which the estimated function is evaluated. * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ @@ -260,8 +264,7 @@ class GTSAM_EXPORT ManifoldEvaluationFactor /** * A unary factor which enforces the evaluation of the derivative of a BASIS - * polynomial is equal to the scalar value of a function at a specified point - * `x`. + * polynomial at a specified point`x` is equal to the scalar measurement `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 */ @@ -279,7 +282,7 @@ class GTSAM_EXPORT DerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The result of the function to estimate evaluated at `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -293,7 +296,7 @@ class GTSAM_EXPORT DerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The result of the function to estimate evaluated at `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -309,8 +312,7 @@ class GTSAM_EXPORT DerivativeFactor /** * A unary factor which enforces the evaluation of the derivative of a BASIS - * polynomial is equal to the vector value of a function at a specified point - * `x`. + * polynomial at a specified point `x` is equal to the vector value `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector derivative. @@ -330,7 +332,7 @@ class GTSAM_EXPORT VectorDerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The vector result of the function to estimate evaluated at `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -345,7 +347,7 @@ class GTSAM_EXPORT VectorDerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The vector result of the function to estimate evaluated at `x`. + * @param z The measurement value. * @param model The noise model. * @param N The degree of the polynomial. * @param x The point at which to evaluate the basis polynomial. @@ -363,7 +365,7 @@ class GTSAM_EXPORT VectorDerivativeFactor /** * A unary factor which enforces the evaluation of the derivative of a BASIS * polynomial is equal to the scalar value at a specific index `i` of a - * vector-valued function at a specified point `x`. + * vector-valued measurement `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 * @param P: Size of the control component derivative. @@ -383,8 +385,8 @@ class GTSAM_EXPORT ComponentDerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The scalar at a specific index `i` of the vector value of the - * function to estimate evaluated at `x`. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. * @param model The degree of the polynomial. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar @@ -401,8 +403,8 @@ class GTSAM_EXPORT ComponentDerivativeFactor * * @param key The key to the ParameterMatrix which represents the basis * polynomial. - * @param z The scalar at specified index `i` of the vector value of the - * function to estimate evaluated at `x`. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. * @param model The degree of the polynomial. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar From 8e759ceba49a33ce101e73e5742d75a7078fc1ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Aug 2021 19:35:08 -0400 Subject: [PATCH 36/36] typo fix --- gtsam/basis/BasisFactors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index cdbdb68476..0b3d4c1a03 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -25,7 +25,7 @@ namespace gtsam { /** * @brief Factor for enforcing the scalar value of the polynomial BASIS - * represenation at `x` is the same as the measurement `z` when using a + * representation at `x` is the same as the measurement `z` when using a * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2