From 42231879bfc9e6075d0f230216197ffda1e4a324 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:47:40 -0400 Subject: [PATCH] remove ParameterMatrix in favor of gtsam::Matrix --- gtsam/basis/Basis.h | 28 ++- gtsam/basis/BasisFactors.h | 48 +++-- gtsam/basis/ParameterMatrix.h | 207 ---------------------- gtsam/basis/basis.i | 11 -- gtsam/basis/tests/testBasisFactors.cpp | 21 ++- gtsam/basis/tests/testChebyshev2.cpp | 33 ++-- gtsam/basis/tests/testFourier.cpp | 7 +- gtsam/basis/tests/testParameterMatrix.cpp | 145 --------------- gtsam/nonlinear/nonlinear.i | 1 - gtsam/nonlinear/values.i | 7 +- 10 files changed, 65 insertions(+), 443 deletions(-) delete mode 100644 gtsam/basis/ParameterMatrix.h delete mode 100644 gtsam/basis/tests/testParameterMatrix.cpp diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index ddc8f4ddd..41cdeeaaa 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -20,7 +20,6 @@ #include #include -#include #include @@ -160,7 +159,7 @@ class Basis { }; /** - * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * VectorEvaluationFunctor at a given x, applied to a parameter Matrix. * 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. @@ -203,14 +202,14 @@ class Basis { } /// M-dimensional evaluation - Vector apply(const ParameterMatrix& P, + Vector apply(const Matrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - Vector operator()(const ParameterMatrix& P, + Vector operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); } @@ -264,21 +263,21 @@ class Basis { } /// Calculate component of component rowIndex_ of P - double apply(const ParameterMatrix& P, + double apply(const Matrix& 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 Matrix& 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 a parameter Matrix. * 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. @@ -307,8 +306,7 @@ class Basis { : Base(M, N, x, a, b) {} /// Manifold evaluation - T apply(const ParameterMatrix& P, - OptionalJacobian H = {}) const { + T apply(const Matrix& P, OptionalJacobian H = {}) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(P, H); @@ -326,7 +324,7 @@ class Basis { } /// c++ sugar - T operator()(const ParameterMatrix& P, + T operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); // might call apply in derived } @@ -382,7 +380,7 @@ class Basis { }; /** - * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * VectorDerivativeFunctor at a given x, applied to a parameter Matrix. * * 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, @@ -426,13 +424,13 @@ class Basis { calculateJacobian(); } - Vector apply(const ParameterMatrix& P, + Vector apply(const Matrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - Vector operator()(const ParameterMatrix& P, + Vector operator()(const Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); } @@ -485,13 +483,13 @@ class Basis { calculateJacobian(N); } /// Calculate derivative of component rowIndex_ of F - double apply(const ParameterMatrix& P, + double apply(const Matrix& 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 Matrix& P, OptionalJacobian H = {}) const { return apply(P, H); } diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 2ce0faae7..ad92ad672 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -75,7 +75,7 @@ class EvaluationFactor : public FunctorizedFactor { }; /** - * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix * of size (M, N) is equal to a vector-valued measurement at the same point, when * using a pseudo-spectral parameterization. @@ -91,10 +91,9 @@ class EvaluationFactor : public FunctorizedFactor { * @ingroup basis */ template -class VectorEvaluationFactor - : public FunctorizedFactor { +class VectorEvaluationFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor; + using Base = FunctorizedFactor; public: VectorEvaluationFactor() {} @@ -102,7 +101,7 @@ class VectorEvaluationFactor /** * @brief Construct a new VectorEvaluationFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The measurement value. * @param model The noise model. @@ -118,7 +117,7 @@ class VectorEvaluationFactor /** * @brief Construct a new VectorEvaluationFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The measurement value. * @param model The noise model. @@ -138,7 +137,7 @@ class VectorEvaluationFactor }; /** - * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix * of size (P, N) is equal to specified measurement at the same point, when * using a pseudo-spectral parameterization. * @@ -156,10 +155,9 @@ class VectorEvaluationFactor * @ingroup basis */ template -class VectorComponentFactor - : public FunctorizedFactor { +class VectorComponentFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor; + using Base = FunctorizedFactor; public: VectorComponentFactor() {} @@ -167,7 +165,7 @@ class VectorComponentFactor /** * @brief Construct a new VectorComponentFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The scalar value at a specified index `i` of the full measurement * vector. @@ -186,7 +184,7 @@ class VectorComponentFactor /** * @brief Construct a new VectorComponentFactor object. * - * @param key The key to the ParameterMatrix object used to represent the + * @param key The key to the parameter Matrix object used to represent the * polynomial. * @param z The scalar value at a specified index `i` of the full measurement * vector. @@ -226,9 +224,9 @@ class VectorComponentFactor * where `x` is the value (e.g. timestep) at which the rotation was evaluated. */ template -class ManifoldEvaluationFactor : public FunctorizedFactor { +class ManifoldEvaluationFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor; + using Base = FunctorizedFactor; public: ManifoldEvaluationFactor() {} @@ -288,7 +286,7 @@ class DerivativeFactor /** * @brief Construct a new DerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -302,7 +300,7 @@ class DerivativeFactor /** * @brief Construct a new DerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -325,10 +323,9 @@ class DerivativeFactor * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -class VectorDerivativeFactor - : public FunctorizedFactor { +class VectorDerivativeFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor; + using Base = FunctorizedFactor; using Func = typename BASIS::VectorDerivativeFunctor; public: @@ -337,7 +334,7 @@ class VectorDerivativeFactor /** * @brief Construct a new VectorDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -353,7 +350,7 @@ class VectorDerivativeFactor /** * @brief Construct a new VectorDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The measurement value. * @param model The noise model. @@ -379,10 +376,9 @@ class VectorDerivativeFactor * @param BASIS: The basis class to use e.g. Chebyshev2 */ template -class ComponentDerivativeFactor - : public FunctorizedFactor { +class ComponentDerivativeFactor : public FunctorizedFactor { private: - using Base = FunctorizedFactor; + using Base = FunctorizedFactor; using Func = typename BASIS::ComponentDerivativeFunctor; public: @@ -391,7 +387,7 @@ class ComponentDerivativeFactor /** * @brief Construct a new ComponentDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. @@ -410,7 +406,7 @@ class ComponentDerivativeFactor /** * @brief Construct a new ComponentDerivativeFactor object. * - * @param key The key to the ParameterMatrix which represents the basis + * @param key The key to the parameter Matrix which represents the basis * polynomial. * @param z The scalar measurement value at a specific index `i` of the full * measurement vector. diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h deleted file mode 100644 index bfb26c4de..000000000 --- a/gtsam/basis/ParameterMatrix.h +++ /dev/null @@ -1,207 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ParameterMatrix.h - * @brief Define ParameterMatrix class which is used to store values at - * interpolation points. - * @author Varun Agrawal, Frank Dellaert - * @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. - * @param N: the number of Basis points (e.g. Chebyshev points of the second - * kind). - */ -class ParameterMatrix { - private: - Matrix matrix_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - enum { dimension = Eigen::Dynamic }; - - /** - * 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 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 Matrix& 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. - Matrix matrix() const { return matrix_; } - - /// Return the tranpose of the underlying matrix. - 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); - } - - /** - * Set the matrix row specified by `index`. - * @param index: The row index to set. - */ - auto row(size_t index) -> Eigen::Block { - 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 column specified by `index`. - * @param index: The column index to set. - */ - auto col(size_t index) -> Eigen::Block { - return matrix_.col(index); - } - - /** - * 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(), 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()); - } - - /** - * 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(), rows(), cols()); - return ParameterMatrix(matrix_ - other_); - } - - /** - * Multiply ParameterMatrix with an Eigen matrix. - * @param other: Eigen matrix which should be multiplication compatible with - * the ParameterMatrix. - */ - Matrix operator*(const Matrix& other) const { return matrix_ * other; } - - /// @name Vector Space requirements - /// @{ - - /** - * 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(size_t M = 0, size_t N = 0) { - return ParameterMatrix(M, N); - } - - /// @} -}; - -/// traits for ParameterMatrix -template <> -struct traits : public internal::VectorSpace { -}; - -/* ************************************************************************* */ -// Stream operator that takes a ParameterMatrix. Used for printing. -inline std::ostream& operator<<(std::ostream& os, - const ParameterMatrix& parameterMatrix) { - os << parameterMatrix.matrix(); - return os; -} - -} // namespace gtsam diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 8cbe4593d..f8cea70f8 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -46,17 +46,6 @@ class Chebyshev2 { static Matrix DifferentiationMatrix(size_t N, double a, double b); }; -#include - -class ParameterMatrix { - ParameterMatrix(const size_t M, const size_t N); - ParameterMatrix(const Matrix& matrix); - - Matrix matrix() const; - - void print(const string& s = "") const; -}; - #include template (key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -133,10 +132,10 @@ TEST(BasisFactors, VectorComponentFactor) { NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix stateMatrix(P, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N); Values initial; - initial.insert(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -158,10 +157,10 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { NonlinearFactorGraph graph; graph.add(factor); - ParameterMatrix stateMatrix(3, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N); Values initial; - initial.insert(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -185,10 +184,10 @@ TEST(BasisFactors, VecDerivativePrior) { NonlinearFactorGraph graph; graph.add(vecDPrior); - ParameterMatrix stateMatrix(M, N); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); Values initial; - initial.insert(key, stateMatrix); + initial.insert(key, stateMatrix); LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); @@ -212,8 +211,8 @@ TEST(BasisFactors, ComponentDerivativeFactor) { graph.add(controlDPrior); Values initial; - ParameterMatrix stateMatrix(M, N); - initial.insert(key, stateMatrix); + gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(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 8aa326ab1..780714936 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 X(2, N); + Matrix X = Matrix::Zero(2, N); X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp // Check value @@ -120,11 +120,11 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - std::function f = + std::function f = std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = - numericalDerivative11(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 X(3, N); + Matrix 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(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 X(6, N); + Matrix X = Matrix::Zero(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(f, X); + numericalDerivative11(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-8)); } #endif @@ -415,12 +415,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { const double x = 0.2; using VecD = Chebyshev2::VectorDerivativeFunctor; VecD fx(M, N, x, 0, 3); - ParameterMatrix X(M, N); + Matrix X = Matrix::Zero(M, N); Matrix actualH(M, M * N); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); // Test Jacobian - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -434,11 +434,10 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { const Vector points = Chebyshev2::Points(N, 0, T); // Assign the parameter matrix 1xN - Matrix values(1, N); + Matrix X(1, N); for (size_t i = 0; i < N; ++i) { - values(i) = f(points(i)); + X(i) = f(points(i)); } - ParameterMatrix X(values); // Evaluate the derivative at the chebyshev points using // VectorDerivativeFunctor. @@ -452,7 +451,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { Matrix actualH(M, M * N); VecD vecd(M, N, points(0), 0, T); vecd(X, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -465,11 +464,11 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; CompFunc fx(M, N, row, x, 0, 3); - ParameterMatrix X(M, N); + Matrix X = Matrix::Zero(M, N); Matrix actualH(1, M * N); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); - Matrix expectedH = numericalDerivative11( + 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 0060dc317..2995e8c45 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -187,10 +187,9 @@ TEST(Basis, VecDerivativeFunctor) { double h = 2 * M_PI / 16; Vector2 dotShape(0.5556, -0.8315); // at 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 theta(theta_mat); + Matrix theta = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); } diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp deleted file mode 100644 index 11a098172..000000000 --- a/gtsam/basis/tests/testParameterMatrix.cpp +++ /dev/null @@ -1,145 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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, Frank Dellaert - * @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 actual1(2, 3); - ParameterMatrix expected1(Matrix::Zero(2, 3)); - EXPECT(assert_equal(expected1, actual1)); - - 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(M, 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(M, 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); - - // row - params.row(0) = Vector::Ones(N); - expected.row(0) = Vector::Ones(N); - EXPECT(assert_equal(expected, params.matrix())); - - // col - params.col(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(M), - ParameterMatrix(Matrix::Zero(M, 0)))); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 19f4ae588..9b1fa58cd 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -25,7 +25,6 @@ namespace gtsam { #include #include #include -#include #include class GraphvizFormatting : gtsam::DotWriter { diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 7ad6c947a..e36b1cf1c 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -25,7 +25,6 @@ namespace gtsam { #include #include #include -#include #include @@ -96,7 +95,6 @@ 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& X); template void insert(size_t j, const T& val); @@ -130,7 +128,6 @@ 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& X); void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point3& point3); @@ -171,7 +168,6 @@ 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& X); template + double}> T at(size_t j); };