12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove template from ParameterMatrix

release/4.3a0
Varun Agrawal 2023-06-19 15:47:18 -04:00
parent 9ae4146ef8
commit 87402328bf
9 changed files with 125 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/basis/Basis.h
View File

@ -169,7 +169,7 @@ class Basis {
}; };
/** /**
* VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>. * VectorEvaluationFunctor at a given x, applied to ParameterMatrix.
* This functor is used to evaluate a parameterized function at a given scalar * 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 * 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. * corresponding functions at x, possibly with Jacobians wrpt the parameters.
@ -211,14 +211,14 @@ class Basis {
} }
/// M-dimensional evaluation /// M-dimensional evaluation
VectorM apply(const ParameterMatrix<M>& P, VectorM apply(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.matrix() * this->weights_.transpose(); return P.matrix() * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
VectorM operator()(const ParameterMatrix<M>& P, VectorM operator()(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
@ -270,21 +270,21 @@ class Basis {
} }
/// Calculate component of component rowIndex_ of P /// Calculate component of component rowIndex_ of P
double apply(const ParameterMatrix<M>& P, double apply(const ParameterMatrix& P,
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
} }
/// c++ sugar /// c++ sugar
double operator()(const ParameterMatrix<M>& P, double operator()(const ParameterMatrix& P,
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
/** /**
* Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>. * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix.
* This functor is used to evaluate a parameterized function at a given scalar * 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 * 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. * corresponding functions at x, possibly with Jacobians wrpt the parameters.
@ -314,7 +314,7 @@ class Basis {
: Base(N, x, a, b) {} : Base(N, x, a, b) {}
/// Manifold evaluation /// Manifold evaluation
T apply(const ParameterMatrix<M>& P, T apply(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const { OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
// Interpolate the M-dimensional vector to yield a vector in tangent space // Interpolate the M-dimensional vector to yield a vector in tangent space
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H); Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
@ -333,7 +333,7 @@ class Basis {
} }
/// c++ sugar /// c++ sugar
T operator()(const ParameterMatrix<M>& P, T operator()(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const { OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H); // might call apply in derived return apply(P, H); // might call apply in derived
} }
@ -389,7 +389,7 @@ class Basis {
}; };
/** /**
* VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>. * VectorDerivativeFunctor at a given x, applied to ParameterMatrix.
* *
* This functor is used to evaluate the derivatives of a parameterized * 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, * function at a given scalar value x. When given a specific M*N parameters,
@ -432,15 +432,14 @@ class Basis {
calculateJacobian(); calculateJacobian();
} }
VectorM apply(const ParameterMatrix<M>& P, VectorM apply(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const { OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.matrix() * this->weights_.transpose(); return P.matrix() * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
VectorM operator()( VectorM operator()(const ParameterMatrix& P,
const ParameterMatrix<M>& P, OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
@ -490,18 +489,17 @@ class Basis {
calculateJacobian(N); calculateJacobian(N);
} }
/// Calculate derivative of component rowIndex_ of F /// Calculate derivative of component rowIndex_ of F
double apply(const ParameterMatrix<M>& P, double apply(const ParameterMatrix& P,
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
if (H) *H = H_; if (H) *H = H_;
return P.row(rowIndex_) * this->weights_.transpose(); return P.row(rowIndex_) * this->weights_.transpose();
} }
/// c++ sugar /// c++ sugar
double operator()(const ParameterMatrix<M>& P, double operator()(const ParameterMatrix& P,
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const { OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H); return apply(P, H);
} }
}; };
}; };
} // namespace gtsam } // namespace gtsam

24
gtsam/basis/BasisFactors.h
View File

@ -93,9 +93,9 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
*/ */
template <class BASIS, int M> template <class BASIS, int M>
class VectorEvaluationFactor class VectorEvaluationFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> { : public FunctorizedFactor<Vector, ParameterMatrix> {
private: private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>; using Base = FunctorizedFactor<Vector, ParameterMatrix>;
public: public:
VectorEvaluationFactor() {} VectorEvaluationFactor() {}
@ -156,11 +156,12 @@ class VectorEvaluationFactor
* *
* @ingroup basis * @ingroup basis
*/ */
// TODO(Varun) remove template P
template <class BASIS, size_t P> template <class BASIS, size_t P>
class VectorComponentFactor class VectorComponentFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> { : public FunctorizedFactor<double, ParameterMatrix> {
private: private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>; using Base = FunctorizedFactor<double, ParameterMatrix>;
public: public:
VectorComponentFactor() {} VectorComponentFactor() {}
@ -226,10 +227,9 @@ class VectorComponentFactor
* where `x` is the value (e.g. timestep) at which the rotation was evaluated. * where `x` is the value (e.g. timestep) at which the rotation was evaluated.
*/ */
template <class BASIS, typename T> template <class BASIS, typename T>
class ManifoldEvaluationFactor class ManifoldEvaluationFactor : public FunctorizedFactor<T, ParameterMatrix> {
: public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
private: private:
using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>; using Base = FunctorizedFactor<T, ParameterMatrix>;
public: public:
ManifoldEvaluationFactor() {} ManifoldEvaluationFactor() {}
@ -326,11 +326,12 @@ class DerivativeFactor
* @param BASIS: The basis class to use e.g. Chebyshev2 * @param BASIS: The basis class to use e.g. Chebyshev2
* @param M: Size of the evaluated state vector derivative. * @param M: Size of the evaluated state vector derivative.
*/ */
//TODO(Varun) remove template M
template <class BASIS, int M> template <class BASIS, int M>
class VectorDerivativeFactor class VectorDerivativeFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> { : public FunctorizedFactor<Vector, ParameterMatrix> {
private: private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>; using Base = FunctorizedFactor<Vector, ParameterMatrix>;
using Func = typename BASIS::template VectorDerivativeFunctor<M>; using Func = typename BASIS::template VectorDerivativeFunctor<M>;
public: public:
@ -379,11 +380,12 @@ class VectorDerivativeFactor
* @param BASIS: The basis class to use e.g. Chebyshev2 * @param BASIS: The basis class to use e.g. Chebyshev2
* @param P: Size of the control component derivative. * @param P: Size of the control component derivative.
*/ */
// TODO(Varun) remove template P
template <class BASIS, int P> template <class BASIS, int P>
class ComponentDerivativeFactor class ComponentDerivativeFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> { : public FunctorizedFactor<double, ParameterMatrix> {
private: private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>; using Base = FunctorizedFactor<double, ParameterMatrix>;
using Func = typename BASIS::template ComponentDerivativeFunctor<P>; using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
public: public:

70
gtsam/basis/ParameterMatrix.h
View File

@ -30,16 +30,12 @@ namespace gtsam {
/** /**
* A matrix abstraction of MxN values at the Basis points. * A matrix abstraction of MxN values at the Basis points.
* This class serves as a wrapper over an Eigen matrix. * 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 * @param N: the number of Basis points (e.g. Chebyshev points of the second
* kind). * kind).
*/ */
template <int M>
class ParameterMatrix { class ParameterMatrix {
using MatrixType = Eigen::Matrix<double, M, -1>;
private: private:
MatrixType matrix_; Matrix matrix_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -48,15 +44,18 @@ class ParameterMatrix {
/** /**
* Create ParameterMatrix using the number of basis points. * 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). * @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. * Create ParameterMatrix from an MxN Eigen Matrix.
* @param matrix: An Eigen matrix used to initialze the ParameterMatrix. * @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. /// Get the number of rows.
size_t rows() const { return matrix_.rows(); } size_t rows() const { return matrix_.rows(); }
@ -65,10 +64,10 @@ class ParameterMatrix {
size_t cols() const { return matrix_.cols(); } size_t cols() const { return matrix_.cols(); }
/// Get the underlying matrix. /// Get the underlying matrix.
MatrixType matrix() const { return matrix_; } Matrix matrix() const { return matrix_; }
/// Return the tranpose of the underlying matrix. /// Return the tranpose of the underlying matrix.
Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); } Matrix transpose() const { return matrix_.transpose(); }
/** /**
* Get the matrix row specified by `index`. * Get the matrix row specified by `index`.
@ -82,7 +81,7 @@ class ParameterMatrix {
* Set the matrix row specified by `index`. * Set the matrix row specified by `index`.
* @param index: The row index to set. * @param index: The row index to set.
*/ */
auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> { auto row(size_t index) -> Eigen::Block<Matrix, 1, -1, false> {
return matrix_.row(index); return matrix_.row(index);
} }
@ -90,7 +89,7 @@ class ParameterMatrix {
* Get the matrix column specified by `index`. * Get the matrix column specified by `index`.
* @param index: The column index to retrieve. * @param index: The column index to retrieve.
*/ */
Eigen::Matrix<double, M, 1> col(size_t index) const { Eigen::Matrix<double, -1, 1> col(size_t index) const {
return matrix_.col(index); return matrix_.col(index);
} }
@ -98,7 +97,7 @@ class ParameterMatrix {
* Set the matrix column specified by `index`. * Set the matrix column specified by `index`.
* @param index: The column index to set. * @param index: The column index to set.
*/ */
auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> { auto col(size_t index) -> Eigen::Block<Matrix, -1, 1, true> {
return matrix_.col(index); return matrix_.col(index);
} }
@ -111,37 +110,35 @@ class ParameterMatrix {
* Add a ParameterMatrix to another. * Add a ParameterMatrix to another.
* @param other: ParameterMatrix to add. * @param other: ParameterMatrix to add.
*/ */
ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const { ParameterMatrix operator+(const ParameterMatrix& other) const {
return ParameterMatrix<M>(matrix_ + other.matrix()); return ParameterMatrix(matrix_ + other.matrix());
} }
/** /**
* Add a MxN-sized vector to the ParameterMatrix. * Add a MxN-sized vector to the ParameterMatrix.
* @param other: Vector which is reshaped and added. * @param other: Vector which is reshaped and added.
*/ */
ParameterMatrix<M> operator+( ParameterMatrix operator+(const Eigen::Matrix<double, -1, 1>& other) const {
const Eigen::Matrix<double, -1, 1>& other) const {
// This form avoids a deep copy and instead typecasts `other`. // This form avoids a deep copy and instead typecasts `other`.
Eigen::Map<const MatrixType> other_(other.data(), M, cols()); Eigen::Map<const Matrix> other_(other.data(), rows(), cols());
return ParameterMatrix<M>(matrix_ + other_); return ParameterMatrix(matrix_ + other_);
} }
/** /**
* Subtract a ParameterMatrix from another. * Subtract a ParameterMatrix from another.
* @param other: ParameterMatrix to subtract. * @param other: ParameterMatrix to subtract.
*/ */
ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const { ParameterMatrix operator-(const ParameterMatrix& other) const {
return ParameterMatrix<M>(matrix_ - other.matrix()); return ParameterMatrix(matrix_ - other.matrix());
} }
/** /**
* Subtract a MxN-sized vector from the ParameterMatrix. * Subtract a MxN-sized vector from the ParameterMatrix.
* @param other: Vector which is reshaped and subracted. * @param other: Vector which is reshaped and subracted.
*/ */
ParameterMatrix<M> operator-( ParameterMatrix operator-(const Eigen::Matrix<double, -1, 1>& other) const {
const Eigen::Matrix<double, -1, 1>& other) const { Eigen::Map<const Matrix> other_(other.data(), rows(), cols());
Eigen::Map<const MatrixType> other_(other.data(), M, cols()); return ParameterMatrix(matrix_ - other_);
return ParameterMatrix<M>(matrix_ - other_);
} }
/** /**
@ -149,9 +146,7 @@ class ParameterMatrix {
* @param other: Eigen matrix which should be multiplication compatible with * @param other: Eigen matrix which should be multiplication compatible with
* the ParameterMatrix. * the ParameterMatrix.
*/ */
MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const { Matrix operator*(const Matrix& other) const { return matrix_ * other; }
return matrix_ * other;
}
/// @name Vector Space requirements /// @name Vector Space requirements
/// @{ /// @{
@ -169,7 +164,7 @@ class ParameterMatrix {
* @param other: The ParameterMatrix to check equality with. * @param other: The ParameterMatrix to check equality with.
* @param tol: The absolute tolerance threshold. * @param tol: The absolute tolerance threshold.
*/ */
bool equals(const ParameterMatrix<M>& 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); 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 * NOTE: The size at compile time is unknown so this identity is zero
* length and thus not valid. * length and thus not valid.
*/ */
inline static ParameterMatrix Identity() { inline static ParameterMatrix Identity(size_t M = 0, size_t N = 0) {
// throw std::runtime_error( return ParameterMatrix(M, N);
// "ParameterMatrix::Identity(): Don't use this function");
return ParameterMatrix(0);
} }
/// @} /// @}
}; };
// traits for ParameterMatrix /// traits for ParameterMatrix
template <int M> template <>
struct traits<ParameterMatrix<M>> struct traits<ParameterMatrix> : public internal::VectorSpace<ParameterMatrix> {
: public internal::VectorSpace<ParameterMatrix<M>> {}; };
/* ************************************************************************* */ /* ************************************************************************* */
// Stream operator that takes a ParameterMatrix. Used for printing. // Stream operator that takes a ParameterMatrix. Used for printing.
template <int M>
inline std::ostream& operator<<(std::ostream& os, inline std::ostream& operator<<(std::ostream& os,
const ParameterMatrix<M>& parameterMatrix) { const ParameterMatrix& parameterMatrix) {
os << parameterMatrix.matrix(); os << parameterMatrix.matrix();
return os; return os;
} }
} // namespace gtsam } // namespace gtsam

3
gtsam/basis/basis.i
View File

@ -48,9 +48,8 @@ class Chebyshev2 {
#include <gtsam/basis/ParameterMatrix.h> #include <gtsam/basis/ParameterMatrix.h>
template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
class ParameterMatrix { class ParameterMatrix {
ParameterMatrix(const size_t N); ParameterMatrix(const size_t M, const size_t N);
ParameterMatrix(const Matrix& matrix); ParameterMatrix(const Matrix& matrix);
Matrix matrix() const; Matrix matrix() const;

51
gtsam/basis/tests/testBasisFactors.cpp
View File

@ -17,30 +17,29 @@
* @brief unit tests for factors in BasisFactors.h * @brief unit tests for factors in BasisFactors.h
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Vector.h>
#include <gtsam/basis/Basis.h> #include <gtsam/basis/Basis.h>
#include <gtsam/basis/BasisFactors.h> #include <gtsam/basis/BasisFactors.h>
#include <gtsam/basis/Chebyshev2.h> #include <gtsam/basis/Chebyshev2.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h> #include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/factorTesting.h> #include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Vector.h>
#include <CppUnitLite/TestHarness.h>
using gtsam::noiseModel::Isotropic;
using gtsam::Pose2;
using gtsam::Vector;
using gtsam::Values;
using gtsam::Chebyshev2; using gtsam::Chebyshev2;
using gtsam::ParameterMatrix;
using gtsam::LevenbergMarquardtParams;
using gtsam::LevenbergMarquardtOptimizer; using gtsam::LevenbergMarquardtOptimizer;
using gtsam::LevenbergMarquardtParams;
using gtsam::NonlinearFactorGraph; using gtsam::NonlinearFactorGraph;
using gtsam::NonlinearOptimizerParams; using gtsam::NonlinearOptimizerParams;
using gtsam::ParameterMatrix;
using gtsam::Pose2;
using gtsam::Values;
using gtsam::Vector;
using gtsam::noiseModel::Isotropic;
constexpr size_t N = 2; constexpr size_t N = 2;
@ -86,10 +85,10 @@ TEST(BasisFactors, VectorEvaluationFactor) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
ParameterMatrix<M> stateMatrix(N); ParameterMatrix stateMatrix(M, N);
Values initial; Values initial;
initial.insert<ParameterMatrix<M>>(key, stateMatrix); initial.insert<ParameterMatrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20); parameters.setMaxIterations(20);
@ -128,16 +127,16 @@ TEST(BasisFactors, VectorComponentFactor) {
const size_t i = 2; const size_t i = 2;
const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
auto model = Isotropic::Sigma(1, 1.0); auto model = Isotropic::Sigma(1, 1.0);
VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i, VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i, t, a,
t, a, b); b);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
ParameterMatrix<P> stateMatrix(N); ParameterMatrix stateMatrix(P, N);
Values initial; Values initial;
initial.insert<ParameterMatrix<P>>(key, stateMatrix); initial.insert<ParameterMatrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20); parameters.setMaxIterations(20);
@ -153,16 +152,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
const Pose2 measured; const Pose2 measured;
const double t = 3.0, a = 2.0, b = 4.0; const double t = 3.0, a = 2.0, b = 4.0;
auto model = Isotropic::Sigma(3, 1.0); auto model = Isotropic::Sigma(3, 1.0);
ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N, ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N, t,
t, a, b); a, b);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
ParameterMatrix<3> stateMatrix(N); ParameterMatrix stateMatrix(3, N);
Values initial; Values initial;
initial.insert<ParameterMatrix<3>>(key, stateMatrix); initial.insert<ParameterMatrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20); parameters.setMaxIterations(20);
@ -186,10 +185,10 @@ TEST(BasisFactors, VecDerivativePrior) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(vecDPrior); graph.add(vecDPrior);
ParameterMatrix<M> stateMatrix(N); ParameterMatrix stateMatrix(M, N);
Values initial; Values initial;
initial.insert<ParameterMatrix<M>>(key, stateMatrix); initial.insert<ParameterMatrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20); parameters.setMaxIterations(20);
@ -213,8 +212,8 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
graph.add(controlDPrior); graph.add(controlDPrior);
Values initial; Values initial;
ParameterMatrix<M> stateMatrix(N); ParameterMatrix stateMatrix(M, N);
initial.insert<ParameterMatrix<M>>(key, stateMatrix); initial.insert<ParameterMatrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20); parameters.setMaxIterations(20);

34
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -108,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) {
double t = 30, a = 0, b = 100; double t = 30, a = 0, b = 100;
const size_t N = 3; const size_t N = 3;
// Create 2x3 matrix with Vectors at Chebyshev points // 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 X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
// Check value // Check value
@ -120,11 +120,11 @@ TEST(Chebyshev2, InterpolateVector) {
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
// Check derivative // Check derivative
std::function<Vector2(ParameterMatrix<2>)> f = std::function<Vector2(ParameterMatrix)> f =
std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X); numericalDerivative11<Vector2, ParameterMatrix, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }
@ -133,7 +133,7 @@ TEST(Chebyshev2, InterpolateVector) {
TEST(Chebyshev2, InterpolatePose2) { TEST(Chebyshev2, InterpolatePose2) {
double t = 30, a = 0, b = 100; 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(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
X.row(1) = Vector::Zero(N); X.row(1) = Vector::Zero(N);
X.row(2) = 0.1 * Vector::Ones(N); X.row(2) = 0.1 * Vector::Ones(N);
@ -148,11 +148,11 @@ TEST(Chebyshev2, InterpolatePose2) {
EXPECT(assert_equal(expected, fx(X, actualH))); EXPECT(assert_equal(expected, fx(X, actualH)));
// Check derivative // Check derivative
std::function<Pose2(ParameterMatrix<3>)> f = std::function<Pose2(ParameterMatrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx, std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH =
numericalDerivative11<Pose2, ParameterMatrix<3>, 3 * N>(f, X); numericalDerivative11<Pose2, ParameterMatrix, 3 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }
@ -169,7 +169,7 @@ TEST(Chebyshev2, InterpolatePose3) {
Vector6 xi = Pose3::ChartAtOrigin::Local(pose); Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N); Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
ParameterMatrix<6> X(N); ParameterMatrix X(6, N);
X.col(11) = xi; X.col(11) = xi;
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b); Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
@ -178,11 +178,11 @@ TEST(Chebyshev2, InterpolatePose3) {
EXPECT(assert_equal(expected, fx(X, actualH))); EXPECT(assert_equal(expected, fx(X, actualH)));
// Check derivative // Check derivative
std::function<Pose3(ParameterMatrix<6>)> f = std::function<Pose3(ParameterMatrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx, std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH =
numericalDerivative11<Pose3, ParameterMatrix<6>, 6 * N>(f, X); numericalDerivative11<Pose3, ParameterMatrix, 6 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-8)); EXPECT(assert_equal(numericalH, actualH, 1e-8));
} }
#endif #endif
@ -415,12 +415,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
const double x = 0.2; const double x = 0.2;
using VecD = Chebyshev2::VectorDerivativeFunctor<M>; using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
VecD fx(N, x, 0, 3); VecD fx(N, x, 0, 3);
ParameterMatrix<M> X(N); ParameterMatrix X(M, N);
Matrix actualH(M, M * N); Matrix actualH(M, M * N);
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
// Test Jacobian // Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix, M * N>(
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }
@ -433,12 +433,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
const Vector points = Chebyshev2::Points(N, 0, T); const Vector points = Chebyshev2::Points(N, 0, T);
// Assign the parameter matrix // Assign the parameter matrix 1xN
Vector values(N); Matrix values(1, N);
for (size_t i = 0; i < N; ++i) { for (size_t i = 0; i < N; ++i) {
values(i) = f(points(i)); values(i) = f(points(i));
} }
ParameterMatrix<M> X(values); ParameterMatrix X(values);
// Evaluate the derivative at the chebyshev points using // Evaluate the derivative at the chebyshev points using
// VectorDerivativeFunctor. // VectorDerivativeFunctor.
@ -452,7 +452,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
Matrix actualH(M, M * N); Matrix actualH(M, M * N);
VecD vecd(N, points(0), 0, T); VecD vecd(N, points(0), 0, T);
vecd(X, actualH); vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix, M * N>(
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-6));
} }
@ -465,11 +465,11 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>; using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
size_t row = 1; size_t row = 1;
CompFunc fx(N, row, x, 0, 3); CompFunc fx(N, row, x, 0, 3);
ParameterMatrix<M> X(N); ParameterMatrix X(M, N);
Matrix actualH(1, M * N); Matrix actualH(1, M * N);
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>( Matrix expectedH = numericalDerivative11<double, ParameterMatrix, M * N>(
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }

2
gtsam/basis/tests/testFourier.cpp
View File

@ -190,7 +190,7 @@ TEST(Basis, VecDerivativeFunctor) {
Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
.finished() .finished()
.transpose(); .transpose();
ParameterMatrix<2> theta(theta_mat); ParameterMatrix theta(theta_mat);
EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
} }

38
gtsam/basis/tests/testParameterMatrix.cpp
View File

@ -32,19 +32,19 @@ const size_t M = 2, N = 5;
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Constructor) { TEST(ParameterMatrix, Constructor) {
ParameterMatrix<2> actual1(3); ParameterMatrix actual1(2, 3);
ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); ParameterMatrix expected1(Matrix::Zero(2, 3));
EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(expected1, actual1));
ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); ParameterMatrix actual2(Matrix::Ones(2, 3));
ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); ParameterMatrix expected2(Matrix::Ones(2, 3));
EXPECT(assert_equal(expected2, actual2)); EXPECT(assert_equal(expected2, actual2));
EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
} }
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Dimensions) { TEST(ParameterMatrix, Dimensions) {
ParameterMatrix<M> params(N); ParameterMatrix params(M, N);
EXPECT_LONGS_EQUAL(params.rows(), M); EXPECT_LONGS_EQUAL(params.rows(), M);
EXPECT_LONGS_EQUAL(params.cols(), N); EXPECT_LONGS_EQUAL(params.cols(), N);
EXPECT_LONGS_EQUAL(params.dim(), M * N); EXPECT_LONGS_EQUAL(params.dim(), M * N);
@ -52,7 +52,7 @@ TEST(ParameterMatrix, Dimensions) {
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Getters) { TEST(ParameterMatrix, Getters) {
ParameterMatrix<M> params(N); ParameterMatrix params(M, N);
Matrix expectedMatrix = Matrix::Zero(2, 5); Matrix expectedMatrix = Matrix::Zero(2, 5);
EXPECT(assert_equal(expectedMatrix, params.matrix())); EXPECT(assert_equal(expectedMatrix, params.matrix()));
@ -60,13 +60,13 @@ TEST(ParameterMatrix, Getters) {
Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
ParameterMatrix<M> p2(Matrix::Ones(M, N)); ParameterMatrix p2(Matrix::Ones(M, N));
Vector expectedRowVector = Vector::Ones(N); Vector expectedRowVector = Vector::Ones(N);
for (size_t r = 0; r < M; ++r) { for (size_t r = 0; r < M; ++r) {
EXPECT(assert_equal(p2.row(r), expectedRowVector)); EXPECT(assert_equal(p2.row(r), expectedRowVector));
} }
ParameterMatrix<M> p3(2 * Matrix::Ones(M, N)); ParameterMatrix p3(2 * Matrix::Ones(M, N));
Vector expectedColVector = 2 * Vector::Ones(M); Vector expectedColVector = 2 * Vector::Ones(M);
for (size_t c = 0; c < M; ++c) { for (size_t c = 0; c < M; ++c) {
EXPECT(assert_equal(p3.col(c), expectedColVector)); EXPECT(assert_equal(p3.col(c), expectedColVector));
@ -75,7 +75,7 @@ TEST(ParameterMatrix, Getters) {
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Setters) { TEST(ParameterMatrix, Setters) {
ParameterMatrix<M> params(Matrix::Zero(M, N)); ParameterMatrix params(Matrix::Zero(M, N));
Matrix expected = Matrix::Zero(M, N); Matrix expected = Matrix::Zero(M, N);
// row // row
@ -97,31 +97,31 @@ TEST(ParameterMatrix, Setters) {
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Addition) { TEST(ParameterMatrix, Addition) {
ParameterMatrix<M> params(Matrix::Ones(M, N)); ParameterMatrix params(Matrix::Ones(M, N));
ParameterMatrix<M> expected(2 * Matrix::Ones(M, N)); ParameterMatrix expected(2 * Matrix::Ones(M, N));
// Add vector // Add vector
EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
// Add another ParameterMatrix // Add another ParameterMatrix
ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N)); ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Subtraction) { TEST(ParameterMatrix, Subtraction) {
ParameterMatrix<M> params(2 * Matrix::Ones(M, N)); ParameterMatrix params(2 * Matrix::Ones(M, N));
ParameterMatrix<M> expected(Matrix::Ones(M, N)); ParameterMatrix expected(Matrix::Ones(M, N));
// Subtract vector // Subtract vector
EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
// Subtract another ParameterMatrix // Subtract another ParameterMatrix
ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N)); ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, Multiplication) { TEST(ParameterMatrix, Multiplication) {
ParameterMatrix<M> params(Matrix::Ones(M, N)); ParameterMatrix params(Matrix::Ones(M, N));
Matrix multiplier = 2 * Matrix::Ones(N, 2); Matrix multiplier = 2 * Matrix::Ones(N, 2);
Matrix expected = 2 * N * Matrix::Ones(M, 2); Matrix expected = 2 * N * Matrix::Ones(M, 2);
EXPECT(assert_equal(expected, params * multiplier)); EXPECT(assert_equal(expected, params * multiplier));
@ -129,12 +129,12 @@ TEST(ParameterMatrix, Multiplication) {
//****************************************************************************** //******************************************************************************
TEST(ParameterMatrix, VectorSpace) { TEST(ParameterMatrix, VectorSpace) {
ParameterMatrix<M> params(Matrix::Ones(M, N)); ParameterMatrix params(Matrix::Ones(M, N));
// vector // vector
EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
// identity // identity
EXPECT(assert_equal(ParameterMatrix<M>::Identity(), EXPECT(assert_equal(ParameterMatrix::Identity(M),
ParameterMatrix<M>(Matrix::Zero(M, 0)))); ParameterMatrix(Matrix::Zero(M, 0))));
} }
//****************************************************************************** //******************************************************************************

64
gtsam/nonlinear/values.i
View File

@ -96,21 +96,7 @@ class Values {
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
void insert(size_t j, double c); 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& 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);
template <T = {gtsam::Point2, gtsam::Point3}> template <T = {gtsam::Point2, gtsam::Point3}>
void insert(size_t j, const T& val); 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, Vector vector);
void update(size_t j, Matrix matrix); void update(size_t j, Matrix matrix);
void update(size_t j, double c); 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& 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 insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point2& point2);
void insert_or_assign(size_t j, const gtsam::Point3& point3); 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, Vector vector);
void insert_or_assign(size_t j, Matrix matrix); 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, double c);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X); void insert_or_assign(size_t j, const gtsam::ParameterMatrix& 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);
template <T = {gtsam::Point2, template <T = {gtsam::Point2,
gtsam::Point3, gtsam::Point3,
@ -244,21 +202,7 @@ class Values {
Vector, Vector,
Matrix, Matrix,
double, double,
gtsam::ParameterMatrix<1>, gtsam::ParameterMatrix}>
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>}>
T at(size_t j); T at(size_t j);
}; };