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

Merge pull request #1537 from borglab/basis-test

release/4.3a0
Frank Dellaert 2023-06-21 20:40:37 -07:00 committed by GitHub
parent b3635cc6ce 42231879bf
commit 68a26a8407
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 314 additions and 658 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

124
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>
@ -81,16 +80,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
@ -169,18 +159,18 @@ class Basis {
};
/**
* VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
* 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.
*/
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 +180,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 +190,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<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
Vector apply(const Matrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(const ParameterMatrix<M>& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
Vector operator()(const Matrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H);
}
};
@ -231,13 +222,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 +240,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,33 +250,34 @@ 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);
}
/// Calculate component of component rowIndex_ of P
double apply(const ParameterMatrix<M>& 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<M>& 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<M>.
* 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.
@ -297,25 +290,23 @@ 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<M>& 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);
@ -333,7 +324,7 @@ class Basis {
}
/// c++ sugar
T operator()(const ParameterMatrix<M>& P,
T operator()(const Matrix& P,
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
return apply(P, H); // might call apply in derived
}
@ -389,20 +380,20 @@ class Basis {
};
/**
* VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
* 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,
* 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 +403,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,25 +413,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<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
Vector apply(const Matrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
if (H) *H = H_;
return P.matrix() * this->weights_.transpose();
}
/// c++ sugar
VectorM operator()(
const ParameterMatrix<M>& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
Vector operator()(const Matrix& P,
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
return apply(P, H);
}
};
@ -452,13 +443,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`.
@ -469,9 +461,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:
@ -479,29 +471,29 @@ 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
double apply(const ParameterMatrix<M>& 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<M>& P,
double operator()(const Matrix& P,
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
return apply(P, H);
}
};
};
} // namespace gtsam

128
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.
@ -87,15 +87,13 @@ 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>
class VectorEvaluationFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
template <class BASIS>
class VectorEvaluationFactor : public FunctorizedFactor<Vector, Matrix> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
using Base = FunctorizedFactor<Vector, Matrix>;
public:
VectorEvaluationFactor() {}
@ -103,42 +101,43 @@ 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.
* @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.
*
* @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.
* @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() {}
};
/**
* 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.
*
@ -147,20 +146,18 @@ 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
*/
template <class BASIS, size_t P>
class VectorComponentFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> {
template <class BASIS>
class VectorComponentFactor : public FunctorizedFactor<double, Matrix> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
using Base = FunctorizedFactor<double, Matrix>;
public:
VectorComponentFactor() {}
@ -168,29 +165,31 @@ 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.
* @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.
*
* @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.
* @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.
@ -199,11 +198,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() {}
};
@ -226,10 +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<traits<T>::dimension>> {
class ManifoldEvaluationFactor : public FunctorizedFactor<T, Matrix> {
private:
using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
using Base = FunctorizedFactor<T, Matrix>;
public:
ManifoldEvaluationFactor() {}
@ -289,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.
@ -303,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.
@ -324,14 +321,12 @@ 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.
*/
template <class BASIS, int M>
class VectorDerivativeFactor
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
template <class BASIS>
class VectorDerivativeFactor : public FunctorizedFactor<Vector, Matrix> {
private:
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
using Func = typename BASIS::template VectorDerivativeFunctor<M>;
using Base = FunctorizedFactor<Vector, Matrix>;
using Func = typename BASIS::VectorDerivativeFunctor;
public:
VectorDerivativeFactor() {}
@ -339,34 +334,36 @@ 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.
* @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.
*
* @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.
* @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() {}
};
@ -377,14 +374,12 @@ class VectorDerivativeFactor
* vector-valued measurement `z`.
*
* @param BASIS: The basis class to use e.g. Chebyshev2
* @param P: Size of the control component derivative.
*/
template <class BASIS, int P>
class ComponentDerivativeFactor
: public FunctorizedFactor<double, ParameterMatrix<P>> {
template <class BASIS>
class ComponentDerivativeFactor : public FunctorizedFactor<double, Matrix> {
private:
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
using Base = FunctorizedFactor<double, Matrix>;
using Func = typename BASIS::ComponentDerivativeFunctor;
public:
ComponentDerivativeFactor() {}
@ -392,29 +387,31 @@ 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.
* @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.
*
* @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.
* @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.
@ -423,9 +420,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() {}
};

215
gtsam/basis/ParameterMatrix.h
View File

@ -1,215 +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.
* @tparam M: The dimension of the type you wish to evaluate.
* @param N: the number of Basis points (e.g. Chebyshev points of the second
* kind).
*/
template <int M>
class ParameterMatrix {
using MatrixType = Eigen::Matrix<double, M, -1>;
private:
MatrixType matrix_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum { dimension = Eigen::Dynamic };
/**
* Create ParameterMatrix using the number of basis points.
* @param N: The number of basis points (the columns).
*/
ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
/**
* Create ParameterMatrix from an MxN Eigen Matrix.
* @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
*/
ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
/// 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.
MatrixType matrix() const { return matrix_; }
/// Return the tranpose of the underlying matrix.
Eigen::Matrix<double, -1, M> 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<MatrixType, 1, -1, false> {
return matrix_.row(index);
}
/**
* Get the matrix column specified by `index`.
* @param index: The column index to retrieve.
*/
Eigen::Matrix<double, M, 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<MatrixType, M, 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<M> operator+(const ParameterMatrix<M>& other) const {
return ParameterMatrix<M>(matrix_ + other.matrix());
}
/**
* Add a MxN-sized vector to the ParameterMatrix.
* @param other: Vector which is reshaped and added.
*/
ParameterMatrix<M> operator+(
const Eigen::Matrix<double, -1, 1>& other) const {
// This form avoids a deep copy and instead typecasts `other`.
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
return ParameterMatrix<M>(matrix_ + other_);
}
/**
* Subtract a ParameterMatrix from another.
* @param other: ParameterMatrix to subtract.
*/
ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
return ParameterMatrix<M>(matrix_ - other.matrix());
}
/**
* Subtract a MxN-sized vector from the ParameterMatrix.
* @param other: Vector which is reshaped and subracted.
*/
ParameterMatrix<M> operator-(
const Eigen::Matrix<double, -1, 1>& other) const {
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
return ParameterMatrix<M>(matrix_ - other_);
}
/**
* Multiply ParameterMatrix with an Eigen matrix.
* @param other: Eigen matrix which should be multiplication compatible with
* the ParameterMatrix.
*/
MatrixType operator*(const Eigen::Matrix<double, -1, -1>& 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<M>& 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() {
// throw std::runtime_error(
// "ParameterMatrix::Identity(): Don't use this function");
return ParameterMatrix(0);
}
/// @}
};
// traits for ParameterMatrix
template <int M>
struct traits<ParameterMatrix<M>>
: public internal::VectorSpace<ParameterMatrix<M>> {};
/* ************************************************************************* */
// Stream operator that takes a ParameterMatrix. Used for printing.
template <int M>
inline std::ostream& operator<<(std::ostream& os,
const ParameterMatrix<M>& parameterMatrix) {
os << parameterMatrix.matrix();
return os;
}
} // namespace gtsam

93
gtsam/basis/basis.i
View File

@ -46,18 +46,6 @@ class Chebyshev2 {
static Matrix DifferentiationMatrix(size_t N, double a, double b);
};
#include <gtsam/basis/ParameterMatrix.h>
template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
class ParameterMatrix {
ParameterMatrix(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,
@ -72,45 +60,36 @@ virtual class EvaluationFactor : gtsam::NoiseModelFactor {
double x, double a, double b);
};
template <BASIS, M>
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
VectorEvaluationFactor();
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
const gtsam::noiseModel::Base* model, const size_t N,
double x);
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x);
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
const gtsam::noiseModel::Base* model, const size_t N,
double x, double a, double b);
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x, double a, double b);
};
// TODO(Varun) Better way to support arbitrary dimensions?
// Especially if users mainly do `pip install gtsam` for the Python wrapper.
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 3>
VectorEvaluationFactorChebyshev2D3;
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
VectorEvaluationFactorChebyshev2D4;
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
VectorEvaluationFactorChebyshev2D12;
template <BASIS, P>
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
VectorComponentFactor();
VectorComponentFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model, const size_t N,
size_t i, double x);
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, size_t i, double x);
VectorComponentFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model, const size_t N,
size_t i, double x, double a, double b);
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, size_t i, double x, double a, double b);
};
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 3>
VectorComponentFactorChebyshev2D3;
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
VectorComponentFactorChebyshev2D4;
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
VectorComponentFactorChebyshev2D12;
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
template <BASIS, T>
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2},
T = {gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
ManifoldEvaluationFactor();
ManifoldEvaluationFactor(gtsam::Key key, const T& z,
@ -121,8 +100,42 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
double x, double a, double b);
};
// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and
// `ComponentDerivativeFactor`
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class DerivativeFactor : gtsam::NoiseModelFactor {
DerivativeFactor();
DerivativeFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model, const size_t N,
double x);
DerivativeFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model, const size_t N,
double x, double a, double b);
};
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor {
VectorDerivativeFactor();
VectorDerivativeFactor(gtsam::Key key, const Vector& z,
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x);
VectorDerivativeFactor(gtsam::Key key, const Vector& z,
const gtsam::noiseModel::Base* model, const size_t M,
const size_t N, double x, double a, double b);
};
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
virtual class ComponentDerivativeFactor : gtsam::NoiseModelFactor {
ComponentDerivativeFactor();
ComponentDerivativeFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model,
const size_t P, const size_t N, size_t i, double x);
ComponentDerivativeFactor(gtsam::Key key, const double z,
const gtsam::noiseModel::Base* model,
const size_t P, const size_t N, size_t i, double x,
double a, double b);
};
#include <gtsam/basis/FitBasis.h>
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,

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

@ -17,30 +17,28 @@
* @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/BasisFactors.h>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.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::ParameterMatrix;
using gtsam::LevenbergMarquardtParams;
using gtsam::LevenbergMarquardtOptimizer;
using gtsam::LevenbergMarquardtParams;
using gtsam::NonlinearFactorGraph;
using gtsam::NonlinearOptimizerParams;
using gtsam::Pose2;
using gtsam::Values;
using gtsam::Vector;
using gtsam::noiseModel::Isotropic;
constexpr size_t N = 2;
@ -81,15 +79,15 @@ 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);
ParameterMatrix<M> stateMatrix(N);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
Values initial;
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);
@ -107,7 +105,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"
@ -128,16 +126,16 @@ TEST(BasisFactors, VectorComponentFactor) {
const size_t i = 2;
const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
auto model = Isotropic::Sigma(1, 1.0);
VectorComponentFactor<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);
ParameterMatrix<P> stateMatrix(N);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N);
Values initial;
initial.insert<ParameterMatrix<P>>(key, stateMatrix);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);
@ -153,16 +151,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
const Pose2 measured;
const double t = 3.0, a = 2.0, b = 4.0;
auto model = Isotropic::Sigma(3, 1.0);
ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N,
t, a, b);
ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N, t,
a, b);
NonlinearFactorGraph graph;
graph.add(factor);
ParameterMatrix<3> stateMatrix(N);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N);
Values initial;
initial.insert<ParameterMatrix<3>>(key, stateMatrix);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);
@ -170,6 +168,8 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, initial, 1e-7, 1e-5);
}
//******************************************************************************
@ -179,15 +179,15 @@ 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);
ParameterMatrix<M> stateMatrix(N);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
Values initial;
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);
@ -204,15 +204,15 @@ 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);
Values initial;
ParameterMatrix<M> stateMatrix(N);
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N);
initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters;
parameters.setMaxIterations(20);

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

@ -17,14 +17,15 @@
* methods
*/
#include <cstddef>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/basis/Chebyshev2.h>
#include <gtsam/basis/FitBasis.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <cstddef>
#include <functional>
using namespace std;
@ -107,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) {
double t = 30, a = 0, b = 100;
const size_t N = 3;
// Create 2x3 matrix with Vectors at Chebyshev points
ParameterMatrix<2> X(N);
Matrix X = Matrix::Zero(2, N);
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
// Check value
@ -115,14 +116,15 @@ 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<2>)> f = std::bind(
&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr);
std::function<Vector2(Matrix)> f =
std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
numericalDerivative11<Vector2, Matrix, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
@ -131,25 +133,66 @@ TEST(Chebyshev2, InterpolateVector) {
TEST(Chebyshev2, InterpolatePose2) {
double t = 30, a = 0, b = 100;
ParameterMatrix<3> X(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);
Vector xi(3);
xi << t, 0, 0.1;
Eigen::Matrix<double, /*3x3N*/ -1, -1> actualH(3, 3 * N);
Chebyshev2::ManifoldEvaluationFunctor<Pose2> fx(N, t, a, b);
// We use xi as canonical coordinates via exponential map
Pose2 expected = Pose2::ChartAtOrigin::Retract(xi);
EXPECT(assert_equal(expected, fx(X)));
EXPECT(assert_equal(expected, fx(X, actualH)));
// Check derivative
std::function<Pose2(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
#ifdef GTSAM_POSE3_EXPMAP
//******************************************************************************
// Interpolating poses using the exponential map
TEST(Chebyshev2, InterpolatePose3) {
double a = 10, b = 100;
double t = Chebyshev2::Points(N, a, b)(11);
Rot3 R = Rot3::Ypr(-2.21366492e-05, -9.35353636e-03, -5.90463598e-04);
Pose3 pose(R, Point3(1, 2, 3));
Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
Matrix X = Matrix::Zero(6, N);
X.col(11) = xi;
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
// We use xi as canonical coordinates via exponential map
Pose3 expected = Pose3::ChartAtOrigin::Retract(xi);
EXPECT(assert_equal(expected, fx(X, actualH)));
// Check derivative
std::function<Pose3(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
std::placeholders::_1, nullptr);
Matrix numericalH =
numericalDerivative11<Pose3, Matrix, 6 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-8));
}
#endif
//******************************************************************************
TEST(Chebyshev2, Decomposition) {
// Create example sequence
Sequence sequence;
for (size_t i = 0; i < 16; i++) {
double x = (1.0/ 16)*i - 0.99, y = x;
double x = (1.0 / 16) * i - 0.99, y = x;
sequence[x] = y;
}
@ -370,14 +413,14 @@ 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);
ParameterMatrix<M> X(N);
using VecD = Chebyshev2::VectorDerivativeFunctor;
VecD fx(M, N, x, 0, 3);
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>, 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));
}
@ -386,30 +429,29 @@ 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);
// Assign the parameter matrix
Vector values(N);
// Assign the parameter matrix 1xN
Matrix X(1, N);
for (size_t i = 0; i < N; ++i) {
values(i) = f(points(i));
X(i) = f(points(i));
}
ParameterMatrix<M> X(values);
// 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>, 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));
}
@ -419,14 +461,14 @@ 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);
ParameterMatrix<M> X(N);
CompFunc fx(M, N, row, x, 0, 3);
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>, 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));
}

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

@ -180,17 +180,16 @@ 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);
Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
.finished()
.transpose();
ParameterMatrix<2> theta(theta_mat);
DotShape dotShapeFunction(2, N, h / 2);
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<2> actual1(3);
ParameterMatrix<2> expected1(Matrix::Zero(2, 3));
EXPECT(assert_equal(expected1, actual1));
ParameterMatrix<2> actual2(Matrix::Ones(2, 3));
ParameterMatrix<2> expected2(Matrix::Ones(2, 3));
EXPECT(assert_equal(expected2, actual2));
EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
}
//******************************************************************************
TEST(ParameterMatrix, Dimensions) {
ParameterMatrix<M> params(N);
EXPECT_LONGS_EQUAL(params.rows(), M);
EXPECT_LONGS_EQUAL(params.cols(), N);
EXPECT_LONGS_EQUAL(params.dim(), M * N);
}
//******************************************************************************
TEST(ParameterMatrix, Getters) {
ParameterMatrix<M> params(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<M> 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<M> 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<M> 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<M> params(Matrix::Ones(M, N));
ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
// Add vector
EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
// Add another ParameterMatrix
ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual));
}
//******************************************************************************
TEST(ParameterMatrix, Subtraction) {
ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
ParameterMatrix<M> expected(Matrix::Ones(M, N));
// Subtract vector
EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
// Subtract another ParameterMatrix
ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
EXPECT(assert_equal(expected, actual));
}
//******************************************************************************
TEST(ParameterMatrix, Multiplication) {
ParameterMatrix<M> 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<M> params(Matrix::Ones(M, N));
// vector
EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
// identity
EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
ParameterMatrix<M>(Matrix::Zero(M, 0))));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

1
gtsam/inference/inference.i
View File

@ -109,6 +109,7 @@ class Ordering {
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
static gtsam::Ordering Colamd(const gtsam::VariableIndex& variableIndex);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,

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 {

63
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,21 +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<1>& X);
void insert(size_t j, const gtsam::ParameterMatrix<2>& X);
void insert(size_t j, const gtsam::ParameterMatrix<3>& X);
void insert(size_t j, const gtsam::ParameterMatrix<4>& X);
void insert(size_t j, const gtsam::ParameterMatrix<5>& X);
void insert(size_t j, const gtsam::ParameterMatrix<6>& X);
void insert(size_t j, const gtsam::ParameterMatrix<7>& X);
void insert(size_t j, const gtsam::ParameterMatrix<8>& X);
void insert(size_t j, const gtsam::ParameterMatrix<9>& X);
void insert(size_t j, const gtsam::ParameterMatrix<10>& X);
void insert(size_t j, const gtsam::ParameterMatrix<11>& X);
void insert(size_t j, const gtsam::ParameterMatrix<12>& X);
void insert(size_t j, const gtsam::ParameterMatrix<13>& X);
void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
template <T = {gtsam::Point2, gtsam::Point3}>
void insert(size_t j, const T& val);
@ -144,21 +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<1>& X);
void update(size_t j, const gtsam::ParameterMatrix<2>& X);
void update(size_t j, const gtsam::ParameterMatrix<3>& X);
void update(size_t j, const gtsam::ParameterMatrix<4>& X);
void update(size_t j, const gtsam::ParameterMatrix<5>& X);
void update(size_t j, const gtsam::ParameterMatrix<6>& X);
void update(size_t j, const gtsam::ParameterMatrix<7>& X);
void update(size_t j, const gtsam::ParameterMatrix<8>& X);
void update(size_t j, const gtsam::ParameterMatrix<9>& X);
void update(size_t j, const gtsam::ParameterMatrix<10>& X);
void update(size_t j, const gtsam::ParameterMatrix<11>& X);
void update(size_t j, const gtsam::ParameterMatrix<12>& X);
void update(size_t j, const gtsam::ParameterMatrix<13>& X);
void update(size_t j, const gtsam::ParameterMatrix<14>& X);
void update(size_t j, const gtsam::ParameterMatrix<15>& X);
void insert_or_assign(size_t j, const gtsam::Point2& point2);
void insert_or_assign(size_t j, const gtsam::Point3& point3);
@ -199,21 +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<1>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X);
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X);
template <T = {gtsam::Point2,
gtsam::Point3,
@ -243,22 +197,7 @@ class Values {
gtsam::NavState,
Vector,
Matrix,
double,
gtsam::ParameterMatrix<1>,
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>}>
double}>
T at(size_t j);
};