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); };