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

remove ParameterMatrix in favor of gtsam::Matrix

release/4.3a0
Varun Agrawal 2023-06-21 17:47:40 -04:00
parent 3ca2ebc48d
commit 42231879bf
10 changed files with 65 additions and 443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/basis/Basis.h
View File

@ -20,7 +20,6 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <iostream>
@ -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</*MxN*/ -1, -1> 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</*MxN*/ -1, -1> 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</*1xMN*/ -1, -1> 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</*1xMN*/ -1, -1> 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</*MxMN*/ -1, -1> H = {}) const {
T apply(const Matrix& P, OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
// Interpolate the M-dimensional vector to yield a vector in tangent space
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
@ -326,7 +324,7 @@ class Basis {
}
/// c++ sugar
T operator()(const ParameterMatrix& P,
T operator()(const Matrix& P,
OptionalJacobian</*MxN*/ -1, -1> 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</*MxMN*/ -1, -1> 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</*MxMN*/ -1, -1> 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</*1xMN*/ -1, -1> 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</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H);
}

48
gtsam/basis/BasisFactors.h
View File

@ -75,7 +75,7 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
};
/**
* 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<double, Vector> {
* @ingroup basis
*/
template <class BASIS>
class VectorEvaluationFactor
: public FunctorizedFactor<Vector, ParameterMatrix> {
class VectorEvaluationFactor : public FunctorizedFactor<Vector, Matrix> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix>;
using Base = FunctorizedFactor<Vector, Matrix>;
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 BASIS>
class VectorComponentFactor
: public FunctorizedFactor<double, ParameterMatrix> {
class VectorComponentFactor : public FunctorizedFactor<double, Matrix> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix>;
using Base = FunctorizedFactor<double, Matrix>;
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 BASIS, typename T>
class ManifoldEvaluationFactor : public FunctorizedFactor<T, ParameterMatrix> {
class ManifoldEvaluationFactor : public FunctorizedFactor<T, Matrix> {
private:
using Base = FunctorizedFactor<T, ParameterMatrix>;
using Base = FunctorizedFactor<T, Matrix>;
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 BASIS>
class VectorDerivativeFactor
: public FunctorizedFactor<Vector, ParameterMatrix> {
class VectorDerivativeFactor : public FunctorizedFactor<Vector, Matrix> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix>;
using Base = FunctorizedFactor<Vector, Matrix>;
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 BASIS>
class ComponentDerivativeFactor
: public FunctorizedFactor<double, ParameterMatrix> {
class ComponentDerivativeFactor : public FunctorizedFactor<double, Matrix> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix>;
using Base = FunctorizedFactor<double, Matrix>;
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.

207
gtsam/basis/ParameterMatrix.h
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/VectorSpace.h>
#include <iostream>
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<double, 1, -1> 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<Matrix, 1, -1, false> {
return matrix_.row(index);
}
/**
* Get the matrix column specified by `index`.
* @param index: The column index to retrieve.
*/
Eigen::Matrix<double, -1, 1> 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<Matrix, -1, 1, true> {
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<double, -1, 1>& other) const {
// This form avoids a deep copy and instead typecasts `other`.
Eigen::Map<const Matrix> 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<double, -1, 1>& other) const {
Eigen::Map<const Matrix> 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<double, -1, -1, Eigen::RowMajor>;
Vector result(matrix_.size());
Eigen::Map<RowMajor>(&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<ParameterMatrix> : public internal::VectorSpace<ParameterMatrix> {
};
/* ************************************************************************* */
// 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

11
gtsam/basis/basis.i
View File

@ -46,17 +46,6 @@ class Chebyshev2 {
static Matrix DifferentiationMatrix(size_t N, double a, double b);
};
#include <gtsam/basis/ParameterMatrix.h>
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 <gtsam/basis/BasisFactors.h>
template <BASIS = {gtsam::Chebyshev2, gtsam::Chebyshev1Basis,

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

@ -35,7 +35,6 @@ using gtsam::LevenbergMarquardtOptimizer;
using gtsam::LevenbergMarquardtParams;
using gtsam::NonlinearFactorGraph;
using gtsam::NonlinearOptimizerParams;
using gtsam::ParameterMatrix;
using gtsam::Pose2;
using gtsam::Values;
using gtsam::Vector;
@ -85,10 +84,10 @@ TEST(BasisFactors, VectorEvaluationFactor) {
NonlinearFactorGraph graph;
graph.add(factor);
ParameterMatrix stateMatrix(M, N);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
Values initial;
initial.insert<ParameterMatrix>(key, stateMatrix);
initial.insert<gtsam::Matrix>(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<ParameterMatrix>(key, stateMatrix);
initial.insert<gtsam::Matrix>(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<ParameterMatrix>(key, stateMatrix);
initial.insert<gtsam::Matrix>(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<ParameterMatrix>(key, stateMatrix);
initial.insert<gtsam::Matrix>(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<ParameterMatrix>(key, stateMatrix);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);

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

@ -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<Vector2(ParameterMatrix)> f =
std::function<Vector2(Matrix)> f =
std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix, 2 * N>(f, X);
numericalDerivative11<Vector2, Matrix, 2 * N>(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<Pose2(ParameterMatrix)> f =
std::function<Pose2(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Pose2, ParameterMatrix, 3 * N>(f, X);
numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
@ -169,7 +169,7 @@ TEST(Chebyshev2, InterpolatePose3) {
Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
ParameterMatrix X(6, N);
Matrix X = Matrix::Zero(6, N);
X.col(11) = xi;
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
@ -178,11 +178,11 @@ TEST(Chebyshev2, InterpolatePose3) {
EXPECT(assert_equal(expected, fx(X, actualH)));
// Check derivative
std::function<Pose3(ParameterMatrix)> f =
std::function<Pose3(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Pose3, ParameterMatrix, 6 * N>(f, X);
numericalDerivative11<Pose3, Matrix, 6 * N>(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<Vector2, ParameterMatrix, M * N>(
Matrix expectedH = numericalDerivative11<Vector2, Matrix, M * N>(
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<Vector1, ParameterMatrix, M * N>(
Matrix expectedH = numericalDerivative11<Vector1, Matrix, M * N>(
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<double, ParameterMatrix, M * N>(
Matrix expectedH = numericalDerivative11<double, Matrix, M * N>(
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7));
}

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

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

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

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/BasisFactors.h>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <gtsam/inference/Symbol.h>
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);
}
//******************************************************************************

1
gtsam/nonlinear/nonlinear.i
View File

@ -25,7 +25,6 @@ namespace gtsam {
#include <gtsam/geometry/Unit3.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <gtsam/nonlinear/GraphvizFormatting.h>
class GraphvizFormatting : gtsam::DotWriter {

7
gtsam/nonlinear/values.i
View File

@ -25,7 +25,6 @@ namespace gtsam {
#include <gtsam/geometry/Unit3.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/basis/ParameterMatrix.h>
#include <gtsam/linear/VectorValues.h>
@ -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 <T = {gtsam::Point2, gtsam::Point3}>
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 <T = {gtsam::Point2,
gtsam::Point3,
@ -201,8 +197,7 @@ class Values {
gtsam::NavState,
Vector,
Matrix,
double,
gtsam::ParameterMatrix}>
double}>
T at(size_t j);
};