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

remove templating and make all Basis code dynamic

release/4.3a0
Varun Agrawal 2023-06-20 08:59:01 -04:00
parent 87402328bf
commit 73c950e69a
6 changed files with 141 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/basis/Basis.cpp Normal file
View File

@ -0,0 +1,33 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Basis.cpp
* @brief Compute an interpolating basis
* @author Varun Agrawal
* @date June 20, 2023
*/
#include <gtsam/basis/Basis.h>
namespace gtsam {
Matrix kroneckerProductIdentity(size_t M, const Weights& w) {
Matrix result(M, w.cols() * M);
result.setZero();
for (int i = 0; i < w.cols(); i++) {
result.block(0, i * M, M, M).diagonal().array() = w(i);
}
return result;
}
} // namespace gtsam

102
gtsam/basis/Basis.h
View File

@ -81,16 +81,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
*
* @ingroup basis
*/
template <size_t M>
Matrix kroneckerProductIdentity(const Weights& w) {
Matrix result(M, w.cols() * M);
result.setZero();
for (int i = 0; i < w.cols(); i++) {
result.block(0, i * M, M, M).diagonal().array() = w(i);
}
return result;
}
Matrix kroneckerProductIdentity(size_t M, const Weights& w);
/**
* CRTP Base class for function bases
@ -174,13 +165,13 @@ class Basis {
* value x. When given a specific M*N parameters, returns an M-vector the M
* corresponding functions at x, possibly with Jacobians wrpt the parameters.
*/
template <int M>
class VectorEvaluationFunctor : protected EvaluationFunctor {
protected:
using VectorM = Eigen::Matrix<double, M, 1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
Jacobian H_;
size_t M_;
/**
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
* the M*N parameter matrix `P`.
@ -190,7 +181,7 @@ class Basis {
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
*/
void calculateJacobian() {
H_ = kroneckerProductIdentity<M>(this->weights_);
H_ = kroneckerProductIdentity(M_, this->weights_);
}
public:
@ -200,26 +191,27 @@ class Basis {
VectorEvaluationFunctor() {}
/// Default Constructor
VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
VectorEvaluationFunctor(size_t M, size_t N, double x)
: EvaluationFunctor(N, x), M_(M) {
calculateJacobian();
}
/// Constructor, with interval [a,b]
VectorEvaluationFunctor(size_t N, double x, double a, double b)
: EvaluationFunctor(N, x, a, b) {
VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
: EvaluationFunctor(N, x, a, b), M_(M) {
calculateJacobian();
}
/// M-dimensional evaluation
VectorM apply(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
Vector apply(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
Vector operator()(const ParameterMatrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H);
}
};
@ -231,13 +223,14 @@ class Basis {
*
* This component is specified by the row index i, with 0<i<M.
*/
template <int M>
class VectorComponentFunctor : public EvaluationFunctor {
protected:
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
size_t rowIndex_;
Jacobian H_;
size_t M_;
size_t rowIndex_;
/*
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
* the M*N parameter matrix `P`.
@ -248,9 +241,9 @@ class Basis {
* MxM identity matrix. See also VectorEvaluationFunctor.
*/
void calculateJacobian(size_t N) {
H_.setZero(1, M * N);
H_.setZero(1, M_ * N);
for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j);
}
public:
@ -258,14 +251,15 @@ class Basis {
VectorComponentFunctor() {}
/// Construct with row index
VectorComponentFunctor(size_t N, size_t i, double x)
: EvaluationFunctor(N, x), rowIndex_(i) {
VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
: EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
calculateJacobian(N);
}
/// Construct with row index and interval
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
: EvaluationFunctor(N, x, a, b), rowIndex_(i) {
VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
double b)
: EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
calculateJacobian(N);
}
@ -297,21 +291,20 @@ class Basis {
* 3D rotation.
*/
template <class T>
class ManifoldEvaluationFunctor
: public VectorEvaluationFunctor<traits<T>::dimension> {
class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
enum { M = traits<T>::dimension };
using Base = VectorEvaluationFunctor<M>;
using Base = VectorEvaluationFunctor;
public:
/// For serialization
ManifoldEvaluationFunctor() {}
/// Default Constructor
ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {}
/// Constructor, with interval [a,b]
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
: Base(N, x, a, b) {}
: Base(M, N, x, a, b) {}
/// Manifold evaluation
T apply(const ParameterMatrix& P,
@ -396,13 +389,13 @@ class Basis {
* returns an M-vector the M corresponding function derivatives at x, possibly
* with Jacobians wrpt the parameters.
*/
template <int M>
class VectorDerivativeFunctor : protected DerivativeFunctorBase {
protected:
using VectorM = Eigen::Matrix<double, M, 1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
Jacobian H_;
size_t M_;
/**
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
* the M*N parameter matrix `P`.
@ -412,7 +405,7 @@ class Basis {
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
*/
void calculateJacobian() {
H_ = kroneckerProductIdentity<M>(this->weights_);
H_ = kroneckerProductIdentity(M_, this->weights_);
}
public:
@ -422,24 +415,25 @@ class Basis {
VectorDerivativeFunctor() {}
/// Default Constructor
VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
VectorDerivativeFunctor(size_t M, size_t N, double x)
: DerivativeFunctorBase(N, x), M_(M) {
calculateJacobian();
}
/// Constructor, with optional interval [a,b]
VectorDerivativeFunctor(size_t N, double x, double a, double b)
: DerivativeFunctorBase(N, x, a, b) {
VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
: DerivativeFunctorBase(N, x, a, b), M_(M) {
calculateJacobian();
}
VectorM apply(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
Vector apply(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
Vector operator()(const ParameterMatrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
return apply(P, H);
}
};
@ -451,13 +445,14 @@ class Basis {
*
* This component is specified by the row index i, with 0<i<M.
*/
template <int M>
class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
protected:
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
size_t rowIndex_;
Jacobian H_;
size_t M_;
size_t rowIndex_;
/*
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
* the M*N parameter matrix `P`.
@ -468,9 +463,9 @@ class Basis {
* MxM identity matrix. See also VectorDerivativeFunctor.
*/
void calculateJacobian(size_t N) {
H_.setZero(1, M * N);
H_.setZero(1, M_ * N);
for (int j = 0; j < this->weights_.size(); j++)
H_(0, rowIndex_ + j * M) = this->weights_(j);
H_(0, rowIndex_ + j * M_) = this->weights_(j);
}
public:
@ -478,14 +473,15 @@ class Basis {
ComponentDerivativeFunctor() {}
/// Construct with row index
ComponentDerivativeFunctor(size_t N, size_t i, double x)
: DerivativeFunctorBase(N, x), rowIndex_(i) {
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
: DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
calculateJacobian(N);
}
/// Construct with row index and interval
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
: DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
double b)
: DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
calculateJacobian(N);
}
/// Calculate derivative of component rowIndex_ of F

82
gtsam/basis/BasisFactors.h
View File

@ -87,11 +87,10 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
* measurement prediction function.
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param M: Size of the evaluated state vector.
*
* @ingroup basis
*/
template <class BASIS, int M>
template <class BASIS>
class VectorEvaluationFactor
: public FunctorizedFactor<Vector, ParameterMatrix> {
private:
@ -107,14 +106,14 @@ class VectorEvaluationFactor
* polynomial.
* @param z The measurement value.
* @param model The noise model.
* @param M Size of the evaluated state vector.
* @param N The degree of the polynomial.
* @param x The point at which to evaluate the basis polynomial.
*/
VectorEvaluationFactor(Key key, const Vector &z,
const SharedNoiseModel &model, const size_t N,
double x)
: Base(key, z, model,
typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
const SharedNoiseModel &model, const size_t M,
const size_t N, double x)
: Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {}
/**
* @brief Construct a new VectorEvaluationFactor object.
@ -123,16 +122,17 @@ class VectorEvaluationFactor
* polynomial.
* @param z The measurement value.
* @param model The noise model.
* @param M Size of the evaluated state vector.
* @param N The degree of the polynomial.
* @param x The point at which to evaluate the basis polynomial.
* @param a Lower bound for the polynomial.
* @param b Upper bound for the polynomial.
*/
VectorEvaluationFactor(Key key, const Vector &z,
const SharedNoiseModel &model, const size_t N,
double x, double a, double b)
const SharedNoiseModel &model, const size_t M,
const size_t N, double x, double a, double b)
: Base(key, z, model,
typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {}
virtual ~VectorEvaluationFactor() {}
};
@ -147,17 +147,15 @@ class VectorEvaluationFactor
* indexed by `i`.
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param P: Size of the fixed-size vector.
*
* Example:
* VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
* N, i, t, a, b);
* VectorComponentFactor<BASIS> controlPrior(key, measured, model,
* N, i, t, a, b);
* where N is the degree and i is the component index.
*
* @ingroup basis
*/
// TODO(Varun) remove template P
template <class BASIS, size_t P>
template <class BASIS>
class VectorComponentFactor
: public FunctorizedFactor<double, ParameterMatrix> {
private:
@ -174,15 +172,16 @@ class VectorComponentFactor
* @param z The scalar value at a specified index `i` of the full measurement
* vector.
* @param model The noise model.
* @param P Size of the fixed-size vector.
* @param N The degree of the polynomial.
* @param i The index for the evaluated vector to give us the desired scalar
* value.
* @param x The point at which to evaluate the basis polynomial.
*/
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
const size_t N, size_t i, double x)
const size_t P, const size_t N, size_t i, double x)
: Base(key, z, model,
typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
typename BASIS::VectorComponentFunctor(P, N, i, x)) {}
/**
* @brief Construct a new VectorComponentFactor object.
@ -192,6 +191,7 @@ class VectorComponentFactor
* @param z The scalar value at a specified index `i` of the full measurement
* vector.
* @param model The noise model.
* @param P Size of the fixed-size vector.
* @param N The degree of the polynomial.
* @param i The index for the evaluated vector to give us the desired scalar
* value.
@ -200,11 +200,10 @@ class VectorComponentFactor
* @param b Upper bound for the polynomial.
*/
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
const size_t N, size_t i, double x, double a, double b)
: Base(
key, z, model,
typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
}
const size_t P, const size_t N, size_t i, double x,
double a, double b)
: Base(key, z, model,
typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {}
virtual ~VectorComponentFactor() {}
};
@ -324,15 +323,13 @@ class DerivativeFactor
* polynomial at a specified point `x` is equal to the vector value `z`.
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param M: Size of the evaluated state vector derivative.
*/
//TODO(Varun) remove template M
template <class BASIS, int M>
template <class BASIS>
class VectorDerivativeFactor
: public FunctorizedFactor<Vector, ParameterMatrix> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix>;
using Func = typename BASIS::template VectorDerivativeFunctor<M>;
using Func = typename BASIS::VectorDerivativeFunctor;
public:
VectorDerivativeFactor() {}
@ -344,13 +341,14 @@ class VectorDerivativeFactor
* polynomial.
* @param z The measurement value.
* @param model The noise model.
* @param M Size of the evaluated state vector derivative.
* @param N The degree of the polynomial.
* @param x The point at which to evaluate the basis polynomial.
*/
VectorDerivativeFactor(Key key, const Vector &z,
const SharedNoiseModel &model, const size_t N,
double x)
: Base(key, z, model, Func(N, x)) {}
const SharedNoiseModel &model, const size_t M,
const size_t N, double x)
: Base(key, z, model, Func(M, N, x)) {}
/**
* @brief Construct a new VectorDerivativeFactor object.
@ -359,15 +357,16 @@ class VectorDerivativeFactor
* polynomial.
* @param z The measurement value.
* @param model The noise model.
* @param M Size of the evaluated state vector derivative.
* @param N The degree of the polynomial.
* @param x The point at which to evaluate the basis polynomial.
* @param a Lower bound for the polynomial.
* @param b Upper bound for the polynomial.
*/
VectorDerivativeFactor(Key key, const Vector &z,
const SharedNoiseModel &model, const size_t N,
double x, double a, double b)
: Base(key, z, model, Func(N, x, a, b)) {}
const SharedNoiseModel &model, const size_t M,
const size_t N, double x, double a, double b)
: Base(key, z, model, Func(M, N, x, a, b)) {}
virtual ~VectorDerivativeFactor() {}
};
@ -378,15 +377,13 @@ class VectorDerivativeFactor
* vector-valued measurement `z`.
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param P: Size of the control component derivative.
*/
// TODO(Varun) remove template P
template <class BASIS, int P>
template <class BASIS>
class ComponentDerivativeFactor
: public FunctorizedFactor<double, ParameterMatrix> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix>;
using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
using Func = typename BASIS::ComponentDerivativeFunctor;
public:
ComponentDerivativeFactor() {}
@ -399,15 +396,16 @@ class ComponentDerivativeFactor
* @param z The scalar measurement value at a specific index `i` of the full
* measurement vector.
* @param model The degree of the polynomial.
* @param P: Size of the control component derivative.
* @param N The degree of the polynomial.
* @param i The index for the evaluated vector to give us the desired scalar
* value.
* @param x The point at which to evaluate the basis polynomial.
*/
ComponentDerivativeFactor(Key key, const double &z,
const SharedNoiseModel &model, const size_t N,
size_t i, double x)
: Base(key, z, model, Func(N, i, x)) {}
const SharedNoiseModel &model, const size_t P,
const size_t N, size_t i, double x)
: Base(key, z, model, Func(P, N, i, x)) {}
/**
* @brief Construct a new ComponentDerivativeFactor object.
@ -417,6 +415,7 @@ class ComponentDerivativeFactor
* @param z The scalar measurement value at a specific index `i` of the full
* measurement vector.
* @param model The degree of the polynomial.
* @param P: Size of the control component derivative.
* @param N The degree of the polynomial.
* @param i The index for the evaluated vector to give us the desired scalar
* value.
@ -425,9 +424,10 @@ class ComponentDerivativeFactor
* @param b Upper bound for the polynomial.
*/
ComponentDerivativeFactor(Key key, const double &z,
const SharedNoiseModel &model, const size_t N,
size_t i, double x, double a, double b)
: Base(key, z, model, Func(N, i, x, a, b)) {}
const SharedNoiseModel &model, const size_t P,
const size_t N, size_t i, double x, double a,
double b)
: Base(key, z, model, Func(P, N, i, x, a, b)) {}
virtual ~ComponentDerivativeFactor() {}
};

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

@ -80,7 +80,7 @@ TEST(BasisFactors, VectorEvaluationFactor) {
const Vector measured = Vector::Zero(M);
auto model = Isotropic::Sigma(M, 1.0);
VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
VectorEvaluationFactor<Chebyshev2> factor(key, measured, model, M, N, 0);
NonlinearFactorGraph graph;
graph.add(factor);
@ -106,7 +106,7 @@ TEST(BasisFactors, Print) {
const Vector measured = Vector::Ones(M) * 42;
auto model = Isotropic::Sigma(M, 1.0);
VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
VectorEvaluationFactor<Chebyshev2> factor(key, measured, model, M, N, 0);
std::string expected =
" keys = { X0 }\n"
@ -127,8 +127,8 @@ TEST(BasisFactors, VectorComponentFactor) {
const size_t i = 2;
const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
auto model = Isotropic::Sigma(1, 1.0);
VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i, t, a,
b);
VectorComponentFactor<Chebyshev2> factor(key, measured, model, P, N, i, t, a,
b);
NonlinearFactorGraph graph;
graph.add(factor);
@ -180,7 +180,7 @@ TEST(BasisFactors, VecDerivativePrior) {
const Vector measured = Vector::Zero(M);
auto model = Isotropic::Sigma(M, 1.0);
VectorDerivativeFactor<Chebyshev2, M> vecDPrior(key, measured, model, N, 0);
VectorDerivativeFactor<Chebyshev2> vecDPrior(key, measured, model, M, N, 0);
NonlinearFactorGraph graph;
graph.add(vecDPrior);
@ -205,8 +205,8 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
double measured = 0;
auto model = Isotropic::Sigma(1, 1.0);
ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model,
N, 0, 0);
ComponentDerivativeFactor<Chebyshev2> controlDPrior(key, measured, model, M,
N, 0, 0);
NonlinearFactorGraph graph;
graph.add(controlDPrior);

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

@ -116,12 +116,12 @@ TEST(Chebyshev2, InterpolateVector) {
expected << t, 0;
Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N);
Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b);
Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b);
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
// Check derivative
std::function<Vector2(ParameterMatrix)> f =
std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx,
std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix, 2 * N>(f, X);
@ -413,8 +413,8 @@ TEST(Chebyshev2, Derivative6_03) {
TEST(Chebyshev2, VectorDerivativeFunctor) {
const size_t N = 3, M = 2;
const double x = 0.2;
using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
VecD fx(N, x, 0, 3);
using VecD = Chebyshev2::VectorDerivativeFunctor;
VecD fx(M, N, x, 0, 3);
ParameterMatrix X(M, N);
Matrix actualH(M, M * N);
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
@ -429,7 +429,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
// Test VectorDerivativeFunctor with polynomial function
TEST(Chebyshev2, VectorDerivativeFunctor2) {
const size_t N = 64, M = 1, T = 15;
using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
using VecD = Chebyshev2::VectorDerivativeFunctor;
const Vector points = Chebyshev2::Points(N, 0, T);
@ -443,14 +443,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
// Evaluate the derivative at the chebyshev points using
// VectorDerivativeFunctor.
for (size_t i = 0; i < N; ++i) {
VecD d(N, points(i), 0, T);
VecD d(M, N, points(i), 0, T);
Vector1 Dx = d(X);
EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
}
// Test Jacobian at the first chebyshev point.
Matrix actualH(M, M * N);
VecD vecd(N, points(0), 0, T);
VecD vecd(M, N, points(0), 0, T);
vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix, M * N>(
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
@ -462,9 +462,9 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
TEST(Chebyshev2, ComponentDerivativeFunctor) {
const size_t N = 6, M = 2;
const double x = 0.2;
using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
using CompFunc = Chebyshev2::ComponentDerivativeFunctor;
size_t row = 1;
CompFunc fx(N, row, x, 0, 3);
CompFunc fx(M, N, row, x, 0, 3);
ParameterMatrix X(M, N);
Matrix actualH(1, M * N);
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);

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

@ -180,13 +180,13 @@ TEST(Basis, Derivative7) {
//******************************************************************************
TEST(Basis, VecDerivativeFunctor) {
using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>;
using DotShape = typename FourierBasis::VectorDerivativeFunctor;
const size_t N = 3;
// MATLAB example, Dec 27 2019, commit 014eded5
double h = 2 * M_PI / 16;
Vector2 dotShape(0.5556, -0.8315); // at h/2
DotShape dotShapeFunction(N, h / 2);
DotShape dotShapeFunction(2, N, h / 2);
Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
.finished()
.transpose();