From 82480fe2383be600069911523e954da6a10f3912 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Jun 2023 15:39:01 -0400 Subject: [PATCH 1/9] added more tests for basis factors --- gtsam/basis/tests/testBasisFactors.cpp | 2 + gtsam/basis/tests/testChebyshev2.cpp | 55 ++++++++++++++++++++++---- 2 files changed, 50 insertions(+), 7 deletions(-) diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index 18a389da5..703830b8d 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -170,6 +170,8 @@ TEST(BasisFactors, ManifoldEvaluationFactor) { LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, initial, 1e-7, 1e-5); } //****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 56a79a38a..e2f01e8af 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -17,14 +17,15 @@ * methods */ -#include +#include +#include #include #include #include +#include #include -#include -#include +#include #include using namespace std; @@ -119,8 +120,9 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - std::function)> f = std::bind( - &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr); + std::function)> f = + std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, + std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); @@ -138,10 +140,49 @@ TEST(Chebyshev2, InterpolatePose2) { Vector xi(3); xi << t, 0, 0.1; + Eigen::Matrix actualH(3, 3 * N); + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); // We use xi as canonical coordinates via exponential map Pose2 expected = Pose2::ChartAtOrigin::Retract(xi); - EXPECT(assert_equal(expected, fx(X))); + EXPECT(assert_equal(expected, fx(X, actualH))); + + // Check derivative + std::function)> f = + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); + Matrix numericalH = + numericalDerivative11, 3 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +// Interpolating poses using the exponential map +TEST(Chebyshev2, InterpolatePose3) { + double a = 10, b = 100; + double t = Chebyshev2::Points(N, a, b)(11); + + Rot3 R = Rot3::Ypr(-2.21366492e-05, -9.35353636e-03, -5.90463598e-04); + Pose3 pose(R, Point3(1, 2, 3)); + + Vector6 xi = Pose3::ChartAtOrigin::Local(pose); + Eigen::Matrix actualH(6, 6 * N); + + ParameterMatrix<6> X(N); + X.col(11) = xi; + + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); + // We use xi as canonical coordinates via exponential map + Pose3 expected = Pose3::ChartAtOrigin::Retract(xi); + EXPECT(assert_equal(expected, fx(X, actualH))); + + // Check derivative + std::function)> f = + std::bind(&Chebyshev2::ManifoldEvaluationFunctor::operator(), fx, + std::placeholders::_1, nullptr); + Matrix numericalH = + numericalDerivative11, 6 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-8)); } //****************************************************************************** @@ -149,7 +190,7 @@ TEST(Chebyshev2, Decomposition) { // Create example sequence Sequence sequence; for (size_t i = 0; i < 16; i++) { - double x = (1.0/ 16)*i - 0.99, y = x; + double x = (1.0 / 16) * i - 0.99, y = x; sequence[x] = y; } From 57578a4793daecb248f3d0118ed8801c130cc70e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Jun 2023 15:42:24 -0400 Subject: [PATCH 2/9] Wrap ManifoldEvaluationFactor for both Rot3 and Pose3 --- gtsam/basis/basis.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 0cd87ba65..5ff4e777c 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -121,6 +121,13 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; +#include + +typedef gtsam::ManifoldEvaluationFactor + ManifoldEvaluationFactorChebyshev2Rot3; +typedef gtsam::ManifoldEvaluationFactor + ManifoldEvaluationFactorChebyshev2Pose3; + // TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and // `ComponentDerivativeFactor` From 87687cee9fad4b5940d19533b5d981e7d55f2cb7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Jun 2023 15:47:02 -0400 Subject: [PATCH 3/9] wrap another Colamd ordering method --- gtsam/inference/inference.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index d4c90e356..953b1da84 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -109,6 +109,7 @@ class Ordering { FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}> static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex); template < FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph, From 9ae4146ef803e2ec01c41abc6681b1b02777cdfb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 6 Jun 2023 18:20:11 -0400 Subject: [PATCH 4/9] conditionally compile Chebyshev2 Pose3 test --- gtsam/basis/tests/testChebyshev2.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index e2f01e8af..e51436fcf 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -156,6 +156,7 @@ TEST(Chebyshev2, InterpolatePose2) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } +#ifdef GTSAM_POSE3_EXPMAP //****************************************************************************** // Interpolating poses using the exponential map TEST(Chebyshev2, InterpolatePose3) { @@ -184,6 +185,7 @@ TEST(Chebyshev2, InterpolatePose3) { numericalDerivative11, 6 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-8)); } +#endif //****************************************************************************** TEST(Chebyshev2, Decomposition) { From 87402328bf2eae8a2b482cb41a14d1061ec55390 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 19 Jun 2023 15:47:18 -0400 Subject: [PATCH 5/9] remove template from ParameterMatrix --- gtsam/basis/Basis.h | 30 +++++----- gtsam/basis/BasisFactors.h | 24 ++++---- gtsam/basis/ParameterMatrix.h | 70 ++++++++++------------- gtsam/basis/basis.i | 3 +- gtsam/basis/tests/testBasisFactors.cpp | 51 ++++++++--------- gtsam/basis/tests/testChebyshev2.cpp | 34 +++++------ gtsam/basis/tests/testFourier.cpp | 2 +- gtsam/basis/tests/testParameterMatrix.cpp | 38 ++++++------ gtsam/nonlinear/values.i | 64 ++------------------- 9 files changed, 125 insertions(+), 191 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 97704dab4..2963b99a8 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -169,7 +169,7 @@ 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. @@ -211,14 +211,14 @@ class Basis { } /// M-dimensional evaluation - VectorM apply(const ParameterMatrix& P, + VectorM apply(const ParameterMatrix& P, OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar - VectorM operator()(const ParameterMatrix& P, + VectorM operator()(const ParameterMatrix& P, OptionalJacobian H = {}) const { return apply(P, H); } @@ -270,21 +270,21 @@ class Basis { } /// 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. @@ -314,7 +314,7 @@ class Basis { : Base(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 +333,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,7 +389,7 @@ 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, @@ -432,15 +432,14 @@ class Basis { calculateJacobian(); } - VectorM apply(const ParameterMatrix& P, + VectorM 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 { + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -490,18 +489,17 @@ class Basis { 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..496d7094c 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -93,9 +93,9 @@ class EvaluationFactor : public FunctorizedFactor { */ template class VectorEvaluationFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorEvaluationFactor() {} @@ -156,11 +156,12 @@ class VectorEvaluationFactor * * @ingroup basis */ +// TODO(Varun) remove template P template class VectorComponentFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; public: VectorComponentFactor() {} @@ -226,10 +227,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() {} @@ -326,11 +326,12 @@ class DerivativeFactor * @param BASIS: The basis class to use e.g. Chebyshev2 * @param M: Size of the evaluated state vector derivative. */ +//TODO(Varun) remove template M template class VectorDerivativeFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; using Func = typename BASIS::template VectorDerivativeFunctor; public: @@ -379,11 +380,12 @@ class VectorDerivativeFactor * @param BASIS: The basis class to use e.g. Chebyshev2 * @param P: Size of the control component derivative. */ +// TODO(Varun) remove template P template class ComponentDerivativeFactor - : public FunctorizedFactor> { + : public FunctorizedFactor { private: - using Base = FunctorizedFactor>; + using Base = FunctorizedFactor; using Func = typename BASIS::template ComponentDerivativeFunctor

; public: 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..61ba1eada 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; diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index 703830b8d..c2552ac11 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -17,30 +17,29 @@ * @brief unit tests for factors in BasisFactors.h */ +#include +#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; @@ -86,10 +85,10 @@ TEST(BasisFactors, VectorEvaluationFactor) { 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); @@ -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, 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); @@ -186,10 +185,10 @@ TEST(BasisFactors, VecDerivativePrior) { 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); @@ -213,8 +212,8 @@ TEST(BasisFactors, ComponentDerivativeFactor) { 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..abf98e8e4 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 @@ -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<2>::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 @@ -415,12 +415,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { const double x = 0.2; using VecD = Chebyshev2::VectorDerivativeFunctor; VecD fx(N, x, 0, 3); - ParameterMatrix X(N); + 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)); } @@ -433,12 +433,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { 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. @@ -452,7 +452,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { Matrix actualH(M, M * N); VecD vecd(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)); } @@ -465,11 +465,11 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { using CompFunc = Chebyshev2::ComponentDerivativeFunctor; size_t row = 1; CompFunc fx(N, row, x, 0, 3); - ParameterMatrix X(N); + 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..c24d4b30a 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -190,7 +190,7 @@ TEST(Basis, VecDerivativeFunctor) { 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); }; From 73c950e69a716c0042622388a48a6f02fb31c767 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 08:59:01 -0400 Subject: [PATCH 6/9] remove templating and make all Basis code dynamic --- gtsam/basis/Basis.cpp | 33 ++++++++ gtsam/basis/Basis.h | 102 ++++++++++++------------- gtsam/basis/BasisFactors.h | 82 ++++++++++---------- gtsam/basis/tests/testBasisFactors.cpp | 14 ++-- gtsam/basis/tests/testChebyshev2.cpp | 18 ++--- gtsam/basis/tests/testFourier.cpp | 4 +- 6 files changed, 141 insertions(+), 112 deletions(-) create mode 100644 gtsam/basis/Basis.cpp 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 2963b99a8..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 @@ -174,13 +165,13 @@ class Basis { * 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,14 +251,15 @@ 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); } @@ -297,21 +291,20 @@ 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, @@ -396,13 +389,13 @@ class Basis { * 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,24 +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); } }; @@ -451,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`. @@ -468,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: @@ -478,14 +473,15 @@ 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 diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 496d7094c..2ce0faae7 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -87,11 +87,10 @@ 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 { private: @@ -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,17 +147,15 @@ 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 */ -// TODO(Varun) remove template P -template +template class VectorComponentFactor : public FunctorizedFactor { private: @@ -174,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. @@ -192,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. @@ -200,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() {} }; @@ -324,15 +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. */ -//TODO(Varun) remove template M -template +template class VectorDerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; - using Func = typename BASIS::template VectorDerivativeFunctor; + using Func = typename BASIS::VectorDerivativeFunctor; public: VectorDerivativeFactor() {} @@ -344,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. @@ -359,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() {} }; @@ -378,15 +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. */ -// TODO(Varun) remove template P -template +template class ComponentDerivativeFactor : public FunctorizedFactor { private: using Base = FunctorizedFactor; - using Func = typename BASIS::template ComponentDerivativeFunctor

; + using Func = typename BASIS::ComponentDerivativeFunctor; public: ComponentDerivativeFactor() {} @@ -399,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. @@ -417,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. @@ -425,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/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index c2552ac11..2f936ddba 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -80,7 +80,7 @@ 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); @@ -106,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" @@ -127,8 +127,8 @@ 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); @@ -180,7 +180,7 @@ 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); @@ -205,8 +205,8 @@ 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); diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index abf98e8e4..8aa326ab1 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -116,12 +116,12 @@ 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::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f, X); @@ -413,8 +413,8 @@ 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); + 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)); @@ -429,7 +429,7 @@ 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); @@ -443,14 +443,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { // 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( std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); @@ -462,9 +462,9 @@ 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); + 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); diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index c24d4b30a..0060dc317 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -180,13 +180,13 @@ 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(); From 445ffb3110cf68de553bbea8b5e0fe10cf824d8f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 09:07:52 -0400 Subject: [PATCH 7/9] update wrapped code --- gtsam/basis/basis.i | 71 +++++++++++++++++++++++++++------------------ 1 file changed, 43 insertions(+), 28 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index 61ba1eada..ce91f7782 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -71,44 +71,28 @@ 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; - template virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { ManifoldEvaluationFactor(); @@ -127,8 +111,39 @@ typedef gtsam::ManifoldEvaluationFactor typedef gtsam::ManifoldEvaluationFactor ManifoldEvaluationFactorChebyshev2Pose3; -// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and -// `ComponentDerivativeFactor` +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); +}; + +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); +}; + +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 Date: Tue, 20 Jun 2023 09:14:12 -0400 Subject: [PATCH 8/9] template on multiple bases --- gtsam/basis/basis.i | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i index ce91f7782..8cbe4593d 100644 --- a/gtsam/basis/basis.i +++ b/gtsam/basis/basis.i @@ -71,7 +71,8 @@ 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, @@ -82,7 +83,8 @@ virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { const size_t N, double x, double a, double b); }; -template +template virtual class VectorComponentFactor : gtsam::NoiseModelFactor { VectorComponentFactor(); VectorComponentFactor(gtsam::Key key, const double z, @@ -93,7 +95,12 @@ virtual class VectorComponentFactor : gtsam::NoiseModelFactor { const size_t N, size_t i, double x, double a, double b); }; -template +#include +#include + +template virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { ManifoldEvaluationFactor(); ManifoldEvaluationFactor(gtsam::Key key, const T& z, @@ -104,14 +111,8 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; -#include - -typedef gtsam::ManifoldEvaluationFactor - ManifoldEvaluationFactorChebyshev2Rot3; -typedef gtsam::ManifoldEvaluationFactor - ManifoldEvaluationFactorChebyshev2Pose3; - -template +template virtual class DerivativeFactor : gtsam::NoiseModelFactor { DerivativeFactor(); DerivativeFactor(gtsam::Key key, const double z, @@ -122,7 +123,8 @@ virtual class DerivativeFactor : gtsam::NoiseModelFactor { double x, double a, double b); }; -template +template virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor { VectorDerivativeFactor(); VectorDerivativeFactor(gtsam::Key key, const Vector& z, @@ -133,7 +135,8 @@ virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor { const size_t N, double x, double a, double b); }; -template +template virtual class ComponentDerivativeFactor : gtsam::NoiseModelFactor { ComponentDerivativeFactor(); ComponentDerivativeFactor(gtsam::Key key, const double z, From 42231879bfc9e6075d0f230216197ffda1e4a324 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:47:40 -0400 Subject: [PATCH 9/9] 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); };