diff --git a/gtsam/basis/Basis.cpp b/gtsam/basis/Basis.cpp new file mode 100644 index 000000000..3e684b197 --- /dev/null +++ b/gtsam/basis/Basis.cpp @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * 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.cpp + * @brief Compute an interpolating basis + * @author Varun Agrawal + * @date June 20, 2023 + */ + +#include + +namespace gtsam { + +Matrix kroneckerProductIdentity(size_t M, const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +} // namespace gtsam diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 97704dab4..ddc8f4ddd 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -81,16 +81,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -template -Matrix kroneckerProductIdentity(const Weights& w) { - Matrix result(M, w.cols() * M); - result.setZero(); - - for (int i = 0; i < w.cols(); i++) { - result.block(0, i * M, M, M).diagonal().array() = w(i); - } - return result; -} +Matrix kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases @@ -169,18 +160,18 @@ class Basis { }; /** - * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * 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 Jacobians wrpt the parameters. */ - template class VectorEvaluationFunctor : protected EvaluationFunctor { protected: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -190,7 +181,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -200,26 +191,27 @@ class Basis { VectorEvaluationFunctor() {} /// Default Constructor - VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + VectorEvaluationFunctor(size_t M, size_t N, double x) + : EvaluationFunctor(N, x), M_(M) { calculateJacobian(); } /// Constructor, with interval [a,b] - VectorEvaluationFunctor(size_t N, double x, double a, double b) - : EvaluationFunctor(N, x, a, b) { + VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), M_(M) { calculateJacobian(); } /// M-dimensional evaluation - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const ParameterMatrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const ParameterMatrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -231,13 +223,14 @@ class Basis { * * This component is specified by the row index i, with 0 class VectorComponentFunctor : public EvaluationFunctor { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -248,9 +241,9 @@ class Basis { * MxM identity matrix. See also VectorEvaluationFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) - H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j); } public: @@ -258,33 +251,34 @@ class Basis { VectorComponentFunctor() {} /// Construct with row index - VectorComponentFunctor(size_t N, size_t i, double x) - : EvaluationFunctor(N, x), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x) + : EvaluationFunctor(N, x), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Construct with row index and interval - VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) - : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Calculate component of component rowIndex_ of P - double apply(const ParameterMatrix& P, + double apply(const ParameterMatrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& P, + double operator()(const ParameterMatrix& P, OptionalJacobian H = {}) const { return apply(P, H); } }; /** - * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * 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 Jacobians wrpt the parameters. @@ -297,24 +291,23 @@ class Basis { * 3D rotation. */ template - class ManifoldEvaluationFunctor - : public VectorEvaluationFunctor::dimension> { + class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { enum { M = traits::dimension }; - using Base = VectorEvaluationFunctor; + using Base = VectorEvaluationFunctor; public: /// For serialization ManifoldEvaluationFunctor() {} /// Default Constructor - ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {} /// Constructor, with interval [a,b] ManifoldEvaluationFunctor(size_t N, double x, double a, double b) - : Base(N, x, a, b) {} + : Base(M, N, x, a, b) {} /// Manifold evaluation - T apply(const ParameterMatrix& P, + T apply(const ParameterMatrix& P, OptionalJacobian H = {}) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(P, H); @@ -333,7 +326,7 @@ class Basis { } /// c++ sugar - T operator()(const ParameterMatrix& P, + T operator()(const ParameterMatrix& P, OptionalJacobian H = {}) const { return apply(P, H); // might call apply in derived } @@ -389,20 +382,20 @@ class Basis { }; /** - * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * 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: - using VectorM = Eigen::Matrix; - using Jacobian = Eigen::Matrix; + using Jacobian = Eigen::Matrix; Jacobian H_; + size_t M_; + /** * Calculate the `M*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -412,7 +405,7 @@ class Basis { * i.e., the Kronecker product of weights_ with the MxM identity matrix. */ void calculateJacobian() { - H_ = kroneckerProductIdentity(this->weights_); + H_ = kroneckerProductIdentity(M_, this->weights_); } public: @@ -422,25 +415,25 @@ class Basis { VectorDerivativeFunctor() {} /// Default Constructor - VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + VectorDerivativeFunctor(size_t M, size_t N, double x) + : DerivativeFunctorBase(N, x), M_(M) { calculateJacobian(); } /// Constructor, with optional interval [a,b] - VectorDerivativeFunctor(size_t N, double x, double a, double b) - : DerivativeFunctorBase(N, x, a, b) { + VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), M_(M) { calculateJacobian(); } - VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector apply(const ParameterMatrix& P, + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()( - const ParameterMatrix& P, - OptionalJacobian H = {}) const { + Vector operator()(const ParameterMatrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -452,13 +445,14 @@ class Basis { * * This component is specified by the row index i, with 0 class ComponentDerivativeFunctor : protected DerivativeFunctorBase { protected: using Jacobian = Eigen::Matrix; - size_t rowIndex_; Jacobian H_; + size_t M_; + size_t rowIndex_; + /* * Calculate the `1*(M*N)` Jacobian of this functor with respect to * the M*N parameter matrix `P`. @@ -469,9 +463,9 @@ class Basis { * MxM identity matrix. See also VectorDerivativeFunctor. */ void calculateJacobian(size_t N) { - H_.setZero(1, M * N); + H_.setZero(1, M_ * N); for (int j = 0; j < this->weights_.size(); j++) - H_(0, rowIndex_ + j * M) = this->weights_(j); + H_(0, rowIndex_ + j * M_) = this->weights_(j); } public: @@ -479,29 +473,29 @@ class Basis { ComponentDerivativeFunctor() {} /// Construct with row index - ComponentDerivativeFunctor(size_t N, size_t i, double x) - : DerivativeFunctorBase(N, x), rowIndex_(i) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), M_(M), 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) { + ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a, + double b) + : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F - double apply(const ParameterMatrix& P, + double apply(const ParameterMatrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar - double operator()(const ParameterMatrix& P, + double operator()(const ParameterMatrix& P, OptionalJacobian H = {}) const { return apply(P, H); } }; - }; } // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 0e42a8866..2ce0faae7 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -87,15 +87,14 @@ class EvaluationFactor : public FunctorizedFactor { * measurement prediction function. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param M: Size of the evaluated state vector. * * @ingroup basis */ -template +template class VectorEvaluationFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorEvaluationFactor() {} @@ -107,14 +106,14 @@ class VectorEvaluationFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @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)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {} /** * @brief Construct a new VectorEvaluationFactor object. @@ -123,16 +122,17 @@ class VectorEvaluationFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector. * @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) + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) : Base(key, z, model, - typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {} virtual ~VectorEvaluationFactor() {} }; @@ -147,20 +147,19 @@ class VectorEvaluationFactor * indexed by `i`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the fixed-size vector. * * Example: - * VectorComponentFactor controlPrior(key, measured, model, - * N, i, t, a, b); + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); * where N is the degree and i is the component index. * * @ingroup basis */ -template +template class VectorComponentFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorComponentFactor() {} @@ -173,15 +172,16 @@ class VectorComponentFactor * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @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) + const size_t P, const size_t N, size_t i, double x) : Base(key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + typename BASIS::VectorComponentFunctor(P, N, i, x)) {} /** * @brief Construct a new VectorComponentFactor object. @@ -191,6 +191,7 @@ class VectorComponentFactor * @param z The scalar value at a specified index `i` of the full measurement * vector. * @param model The noise model. + * @param P Size of the fixed-size vector. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -199,11 +200,10 @@ class VectorComponentFactor * @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( - key, z, model, - typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { - } + const size_t P, const size_t N, size_t i, double x, + double a, double b) + : Base(key, z, model, + typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {} virtual ~VectorComponentFactor() {} }; @@ -226,10 +226,9 @@ class VectorComponentFactor * where `x` is the value (e.g. timestep) at which the rotation was evaluated. */ template -class ManifoldEvaluationFactor - : public FunctorizedFactor::dimension>> { +class ManifoldEvaluationFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor::dimension>>; + using Base = FunctorizedFactor; public: ManifoldEvaluationFactor() {} @@ -324,14 +323,13 @@ class DerivativeFactor * 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. */ -template +template class VectorDerivativeFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; - using Func = typename BASIS::template VectorDerivativeFunctor; + using Base = FunctorizedFactor; + using Func = typename BASIS::VectorDerivativeFunctor; public: VectorDerivativeFactor() {} @@ -343,13 +341,14 @@ class VectorDerivativeFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @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)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x) + : Base(key, z, model, Func(M, N, x)) {} /** * @brief Construct a new VectorDerivativeFactor object. @@ -358,15 +357,16 @@ class VectorDerivativeFactor * polynomial. * @param z The measurement value. * @param model The noise model. + * @param M Size of the evaluated state vector derivative. * @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) - : Base(key, z, model, Func(N, x, a, b)) {} + const SharedNoiseModel &model, const size_t M, + const size_t N, double x, double a, double b) + : Base(key, z, model, Func(M, N, x, a, b)) {} virtual ~VectorDerivativeFactor() {} }; @@ -377,14 +377,13 @@ class VectorDerivativeFactor * vector-valued measurement `z`. * * @param BASIS: The basis class to use e.g. Chebyshev2 - * @param P: Size of the control component derivative. */ -template +template class ComponentDerivativeFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; - using Func = typename BASIS::template ComponentDerivativeFunctor

; + using Base = FunctorizedFactor; + using Func = typename BASIS::ComponentDerivativeFunctor; public: ComponentDerivativeFactor() {} @@ -397,15 +396,16 @@ class ComponentDerivativeFactor * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @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)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x) + : Base(key, z, model, Func(P, N, i, x)) {} /** * @brief Construct a new ComponentDerivativeFactor object. @@ -415,6 +415,7 @@ class ComponentDerivativeFactor * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. * @param model The degree of the polynomial. + * @param P: Size of the control component derivative. * @param N The degree of the polynomial. * @param i The index for the evaluated vector to give us the desired scalar * value. @@ -423,9 +424,10 @@ class ComponentDerivativeFactor * @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) - : Base(key, z, model, Func(N, i, x, a, b)) {} + const SharedNoiseModel &model, const size_t P, + const size_t N, size_t i, double x, double a, + double b) + : Base(key, z, model, Func(P, N, i, x, a, b)) {} virtual ~ComponentDerivativeFactor() {} }; diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index 81686e046..bfb26c4de 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -30,16 +30,12 @@ namespace gtsam { /** * A matrix abstraction of MxN values at the Basis points. * This class serves as a wrapper over an Eigen matrix. - * @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). */ -template class ParameterMatrix { - using MatrixType = Eigen::Matrix; - private: - MatrixType matrix_; + Matrix matrix_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -48,15 +44,18 @@ class ParameterMatrix { /** * Create ParameterMatrix using the number of basis points. + * @param M: The dimension size of the type you wish to evaluate. * @param N: The number of basis points (the columns). */ - ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + ParameterMatrix(const size_t M, 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) {} + ParameterMatrix(const Matrix& matrix) : matrix_(matrix) {} /// Get the number of rows. size_t rows() const { return matrix_.rows(); } @@ -65,10 +64,10 @@ class ParameterMatrix { size_t cols() const { return matrix_.cols(); } /// Get the underlying matrix. - MatrixType matrix() const { return matrix_; } + Matrix matrix() const { return matrix_; } /// Return the tranpose of the underlying matrix. - Eigen::Matrix transpose() const { return matrix_.transpose(); } + Matrix transpose() const { return matrix_.transpose(); } /** * Get the matrix row specified by `index`. @@ -82,7 +81,7 @@ class ParameterMatrix { * Set the matrix row specified by `index`. * @param index: The row index to set. */ - auto row(size_t index) -> Eigen::Block { + auto row(size_t index) -> Eigen::Block { return matrix_.row(index); } @@ -90,7 +89,7 @@ class ParameterMatrix { * Get the matrix column specified by `index`. * @param index: The column index to retrieve. */ - Eigen::Matrix col(size_t index) const { + Eigen::Matrix col(size_t index) const { return matrix_.col(index); } @@ -98,7 +97,7 @@ class ParameterMatrix { * Set the matrix column specified by `index`. * @param index: The column index to set. */ - auto col(size_t index) -> Eigen::Block { + auto col(size_t index) -> Eigen::Block { return matrix_.col(index); } @@ -111,37 +110,35 @@ class ParameterMatrix { * Add a ParameterMatrix to another. * @param other: ParameterMatrix to add. */ - ParameterMatrix operator+(const ParameterMatrix& other) const { - return ParameterMatrix(matrix_ + other.matrix()); + 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 { + 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_); + Eigen::Map other_(other.data(), rows(), 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()); + 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_); + ParameterMatrix operator-(const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), rows(), cols()); + return ParameterMatrix(matrix_ - other_); } /** @@ -149,9 +146,7 @@ class ParameterMatrix { * @param other: Eigen matrix which should be multiplication compatible with * the ParameterMatrix. */ - MatrixType operator*(const Eigen::Matrix& other) const { - return matrix_ * other; - } + Matrix operator*(const Matrix& other) const { return matrix_ * other; } /// @name Vector Space requirements /// @{ @@ -169,7 +164,7 @@ class ParameterMatrix { * @param other: The ParameterMatrix to check equality with. * @param tol: The absolute tolerance threshold. */ - bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); } @@ -189,27 +184,24 @@ class ParameterMatrix { * 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); + inline static ParameterMatrix Identity(size_t M = 0, size_t N = 0) { + return ParameterMatrix(M, N); } /// @} }; -// traits for ParameterMatrix -template -struct traits> - : public internal::VectorSpace> {}; +/// 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) { + const ParameterMatrix& parameterMatrix) { os << parameterMatrix.matrix(); return os; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 5ff4e777c..8cbe4593d 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -48,9 +48,8 @@ class Chebyshev2 { #include -template class ParameterMatrix { - ParameterMatrix(const size_t N); + ParameterMatrix(const size_t M, const size_t N); ParameterMatrix(const Matrix& matrix); Matrix matrix() const; @@ -72,45 +71,36 @@ virtual class EvaluationFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; -template +template virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { VectorEvaluationFactor(); VectorEvaluationFactor(gtsam::Key key, const Vector& z, - const gtsam::noiseModel::Base* model, const size_t N, - double x); + const gtsam::noiseModel::Base* model, const size_t M, + 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); + const gtsam::noiseModel::Base* model, const size_t M, + 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 +template virtual 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); + const gtsam::noiseModel::Base* model, const size_t M, + 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); + const gtsam::noiseModel::Base* model, const size_t M, + 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; +#include +#include -template +template virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { ManifoldEvaluationFactor(); ManifoldEvaluationFactor(gtsam::Key key, const T& z, @@ -121,15 +111,42 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; -#include +template +virtual class DerivativeFactor : gtsam::NoiseModelFactor { + DerivativeFactor(); + DerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + DerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; -typedef gtsam::ManifoldEvaluationFactor - ManifoldEvaluationFactorChebyshev2Rot3; -typedef gtsam::ManifoldEvaluationFactor - ManifoldEvaluationFactorChebyshev2Pose3; +template +virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor { + VectorDerivativeFactor(); + VectorDerivativeFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x); + VectorDerivativeFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t M, + const size_t N, double x, double a, double b); +}; -// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and -// `ComponentDerivativeFactor` +template +virtual class ComponentDerivativeFactor : gtsam::NoiseModelFactor { + ComponentDerivativeFactor(); + ComponentDerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, + const size_t P, const size_t N, size_t i, double x); + ComponentDerivativeFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, + const size_t P, const size_t N, size_t i, double x, + double a, double b); +}; #include template +#include +#include +#include #include #include #include #include +#include #include #include #include -#include -#include -#include -#include -#include - -using gtsam::noiseModel::Isotropic; -using gtsam::Pose2; -using gtsam::Vector; -using gtsam::Values; using gtsam::Chebyshev2; -using gtsam::ParameterMatrix; -using gtsam::LevenbergMarquardtParams; using gtsam::LevenbergMarquardtOptimizer; +using gtsam::LevenbergMarquardtParams; using gtsam::NonlinearFactorGraph; using gtsam::NonlinearOptimizerParams; +using gtsam::ParameterMatrix; +using gtsam::Pose2; +using gtsam::Values; +using gtsam::Vector; +using gtsam::noiseModel::Isotropic; constexpr size_t N = 2; @@ -81,15 +80,15 @@ TEST(BasisFactors, VectorEvaluationFactor) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix stateMatrix(N); + ParameterMatrix stateMatrix(M, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -107,7 +106,7 @@ TEST(BasisFactors, Print) { const Vector measured = Vector::Ones(M) * 42; auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor factor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, M, N, 0); std::string expected = " keys = { X0 }\n" @@ -128,16 +127,16 @@ TEST(BasisFactors, VectorComponentFactor) { const size_t i = 2; const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(1, 1.0); - VectorComponentFactor factor(key, measured, model, N, i, - t, a, b); + VectorComponentFactor factor(key, measured, model, P, N, i, t, a, + b); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix

stateMatrix(N); + ParameterMatrix stateMatrix(P, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -153,16 +152,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { const Pose2 measured; const double t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(3, 1.0); - ManifoldEvaluationFactor factor(key, measured, model, N, - t, a, b); + ManifoldEvaluationFactor factor(key, measured, model, N, t, + a, b); NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix<3> stateMatrix(N); + ParameterMatrix stateMatrix(3, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -181,15 +180,15 @@ TEST(BasisFactors, VecDerivativePrior) { const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + VectorDerivativeFactor vecDPrior(key, measured, model, M, N, 0); NonlinearFactorGraph graph; graph.add(vecDPrior); - ParameterMatrix stateMatrix(N); + ParameterMatrix stateMatrix(M, N); Values initial; - initial.insert>(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -206,15 +205,15 @@ TEST(BasisFactors, ComponentDerivativeFactor) { double measured = 0; auto model = Isotropic::Sigma(1, 1.0); - ComponentDerivativeFactor controlDPrior(key, measured, model, - N, 0, 0); + ComponentDerivativeFactor controlDPrior(key, measured, model, M, + N, 0, 0); NonlinearFactorGraph graph; graph.add(controlDPrior); Values initial; - ParameterMatrix stateMatrix(N); - initial.insert>(key, stateMatrix); + ParameterMatrix stateMatrix(M, N); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index e51436fcf..8aa326ab1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -108,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) { double t = 30, a = 0, b = 100; const size_t N = 3; // Create 2x3 matrix with Vectors at Chebyshev points - ParameterMatrix<2> X(N); + ParameterMatrix X(2, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp // Check value @@ -116,15 +116,15 @@ TEST(Chebyshev2, InterpolateVector) { expected << t, 0; Eigen::Matrix actualH(2, 2 * N); - Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - std::function)> f = - std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, + std::function f = + std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11, 2 * N>(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -133,7 +133,7 @@ TEST(Chebyshev2, InterpolateVector) { TEST(Chebyshev2, InterpolatePose2) { double t = 30, a = 0, b = 100; - ParameterMatrix<3> X(N); + ParameterMatrix X(3, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp X.row(1) = Vector::Zero(N); X.row(2) = 0.1 * Vector::Ones(N); @@ -148,11 +148,11 @@ TEST(Chebyshev2, InterpolatePose2) { EXPECT(assert_equal(expected, fx(X, actualH))); // Check derivative - std::function)> f = + std::function f = std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11, 3 * N>(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } @@ -169,7 +169,7 @@ TEST(Chebyshev2, InterpolatePose3) { Vector6 xi = Pose3::ChartAtOrigin::Local(pose); Eigen::Matrix actualH(6, 6 * N); - ParameterMatrix<6> X(N); + ParameterMatrix X(6, N); X.col(11) = xi; Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); @@ -178,11 +178,11 @@ TEST(Chebyshev2, InterpolatePose3) { EXPECT(assert_equal(expected, fx(X, actualH))); // Check derivative - std::function)> f = + std::function f = std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11, 6 * N>(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-8)); } #endif @@ -413,14 +413,14 @@ TEST(Chebyshev2, Derivative6_03) { TEST(Chebyshev2, VectorDerivativeFunctor) { const size_t N = 3, M = 2; const double x = 0.2; - using VecD = Chebyshev2::VectorDerivativeFunctor; - VecD fx(N, x, 0, 3); - ParameterMatrix X(N); + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(M, N, x, 0, 3); + ParameterMatrix X(M, N); Matrix actualH(M, M * N); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); // Test Jacobian - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -429,30 +429,30 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test VectorDerivativeFunctor with polynomial function TEST(Chebyshev2, VectorDerivativeFunctor2) { const size_t N = 64, M = 1, T = 15; - using VecD = Chebyshev2::VectorDerivativeFunctor; + using VecD = Chebyshev2::VectorDerivativeFunctor; const Vector points = Chebyshev2::Points(N, 0, T); - // Assign the parameter matrix - Vector values(N); + // Assign the parameter matrix 1xN + Matrix values(1, N); for (size_t i = 0; i < N; ++i) { values(i) = f(points(i)); } - ParameterMatrix X(values); + 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); + VecD d(M, 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 vecd(M, N, points(0), 0, T); vecd(X, actualH); - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -462,14 +462,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { TEST(Chebyshev2, ComponentDerivativeFunctor) { const size_t N = 6, M = 2; const double x = 0.2; - using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; - CompFunc fx(N, row, x, 0, 3); - ParameterMatrix X(N); + CompFunc fx(M, N, row, x, 0, 3); + ParameterMatrix X(M, N); Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11, M * N>( + Matrix expectedH = numericalDerivative11( std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index 3a147a9f6..0060dc317 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -180,17 +180,17 @@ TEST(Basis, Derivative7) { //****************************************************************************** TEST(Basis, VecDerivativeFunctor) { - using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + using DotShape = typename FourierBasis::VectorDerivativeFunctor; 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); + DotShape dotShapeFunction(2, N, h / 2); Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) .finished() .transpose(); - ParameterMatrix<2> theta(theta_mat); + ParameterMatrix theta(theta_mat); EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); } diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp index ae2e8e111..11a098172 100644 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -32,19 +32,19 @@ const size_t M = 2, N = 5; //****************************************************************************** TEST(ParameterMatrix, Constructor) { - ParameterMatrix<2> actual1(3); - ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + ParameterMatrix actual1(2, 3); + ParameterMatrix expected1(Matrix::Zero(2, 3)); EXPECT(assert_equal(expected1, actual1)); - ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); - ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + ParameterMatrix actual2(Matrix::Ones(2, 3)); + ParameterMatrix expected2(Matrix::Ones(2, 3)); EXPECT(assert_equal(expected2, actual2)); EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); } //****************************************************************************** TEST(ParameterMatrix, Dimensions) { - ParameterMatrix params(N); + ParameterMatrix params(M, N); EXPECT_LONGS_EQUAL(params.rows(), M); EXPECT_LONGS_EQUAL(params.cols(), N); EXPECT_LONGS_EQUAL(params.dim(), M * N); @@ -52,7 +52,7 @@ TEST(ParameterMatrix, Dimensions) { //****************************************************************************** TEST(ParameterMatrix, Getters) { - ParameterMatrix params(N); + ParameterMatrix params(M, N); Matrix expectedMatrix = Matrix::Zero(2, 5); EXPECT(assert_equal(expectedMatrix, params.matrix())); @@ -60,13 +60,13 @@ TEST(ParameterMatrix, Getters) { Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); - ParameterMatrix p2(Matrix::Ones(M, N)); + 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)); + 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)); @@ -75,7 +75,7 @@ TEST(ParameterMatrix, Getters) { //****************************************************************************** TEST(ParameterMatrix, Setters) { - ParameterMatrix params(Matrix::Zero(M, N)); + ParameterMatrix params(Matrix::Zero(M, N)); Matrix expected = Matrix::Zero(M, N); // row @@ -97,31 +97,31 @@ TEST(ParameterMatrix, Setters) { //****************************************************************************** TEST(ParameterMatrix, Addition) { - ParameterMatrix params(Matrix::Ones(M, N)); - ParameterMatrix expected(2 * Matrix::Ones(M, N)); + 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)); + 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)); + 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)); + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); EXPECT(assert_equal(expected, actual)); } //****************************************************************************** TEST(ParameterMatrix, Multiplication) { - ParameterMatrix params(Matrix::Ones(M, N)); + 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)); @@ -129,12 +129,12 @@ TEST(ParameterMatrix, Multiplication) { //****************************************************************************** TEST(ParameterMatrix, VectorSpace) { - ParameterMatrix params(Matrix::Ones(M, N)); + 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)))); + EXPECT(assert_equal(ParameterMatrix::Identity(M), + ParameterMatrix(Matrix::Zero(M, 0)))); } //****************************************************************************** diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 680cafc31..7ad6c947a 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -96,21 +96,7 @@ class Values { void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); - void insert(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert(size_t j, const gtsam::ParameterMatrix<15>& X); + void insert(size_t j, const gtsam::ParameterMatrix& X); template void insert(size_t j, const T& val); @@ -144,21 +130,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); void update(size_t j, double c); - void update(size_t j, const gtsam::ParameterMatrix<1>& X); - void update(size_t j, const gtsam::ParameterMatrix<2>& X); - void update(size_t j, const gtsam::ParameterMatrix<3>& X); - void update(size_t j, const gtsam::ParameterMatrix<4>& X); - void update(size_t j, const gtsam::ParameterMatrix<5>& X); - void update(size_t j, const gtsam::ParameterMatrix<6>& X); - void update(size_t j, const gtsam::ParameterMatrix<7>& X); - void update(size_t j, const gtsam::ParameterMatrix<8>& X); - void update(size_t j, const gtsam::ParameterMatrix<9>& X); - void update(size_t j, const gtsam::ParameterMatrix<10>& X); - void update(size_t j, const gtsam::ParameterMatrix<11>& X); - void update(size_t j, const gtsam::ParameterMatrix<12>& X); - void update(size_t j, const gtsam::ParameterMatrix<13>& X); - void update(size_t j, const gtsam::ParameterMatrix<14>& X); - void update(size_t j, const gtsam::ParameterMatrix<15>& X); + void update(size_t j, const gtsam::ParameterMatrix& X); void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point3& point3); @@ -199,21 +171,7 @@ class Values { void insert_or_assign(size_t j, Vector vector); void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, double c); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X); - void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X); + void insert_or_assign(size_t j, const gtsam::ParameterMatrix& X); template , - gtsam::ParameterMatrix<2>, - gtsam::ParameterMatrix<3>, - gtsam::ParameterMatrix<4>, - gtsam::ParameterMatrix<5>, - gtsam::ParameterMatrix<6>, - gtsam::ParameterMatrix<7>, - gtsam::ParameterMatrix<8>, - gtsam::ParameterMatrix<9>, - gtsam::ParameterMatrix<10>, - gtsam::ParameterMatrix<11>, - gtsam::ParameterMatrix<12>, - gtsam::ParameterMatrix<13>, - gtsam::ParameterMatrix<14>, - gtsam::ParameterMatrix<15>}> + gtsam::ParameterMatrix}> T at(size_t j); };