Merge pull request #1548 from borglab/basis-dynamic
commit
3ca2ebc48d
|
@ -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
|
|
@ -81,16 +81,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
|
||||||
*
|
*
|
||||||
* @ingroup basis
|
* @ingroup basis
|
||||||
*/
|
*/
|
||||||
template <size_t M>
|
Matrix kroneckerProductIdentity(size_t M, const Weights& w);
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* CRTP Base class for function bases
|
* CRTP Base class for function bases
|
||||||
|
@ -169,18 +160,18 @@ class Basis {
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
|
* VectorEvaluationFunctor at a given x, applied to ParameterMatrix.
|
||||||
* This functor is used to evaluate a parameterized function at a given scalar
|
* This functor is used to evaluate a parameterized function at a given scalar
|
||||||
* value x. When given a specific M*N parameters, returns an M-vector the M
|
* value x. When given a specific M*N parameters, returns an M-vector the M
|
||||||
* corresponding functions at x, possibly with Jacobians wrpt the parameters.
|
* corresponding functions at x, possibly with Jacobians wrpt the parameters.
|
||||||
*/
|
*/
|
||||||
template <int M>
|
|
||||||
class VectorEvaluationFunctor : protected EvaluationFunctor {
|
class VectorEvaluationFunctor : protected EvaluationFunctor {
|
||||||
protected:
|
protected:
|
||||||
using VectorM = Eigen::Matrix<double, M, 1>;
|
using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
|
||||||
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
|
|
||||||
Jacobian H_;
|
Jacobian H_;
|
||||||
|
|
||||||
|
size_t M_;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
|
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
|
||||||
* the M*N parameter matrix `P`.
|
* the M*N parameter matrix `P`.
|
||||||
|
@ -190,7 +181,7 @@ class Basis {
|
||||||
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
|
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
|
||||||
*/
|
*/
|
||||||
void calculateJacobian() {
|
void calculateJacobian() {
|
||||||
H_ = kroneckerProductIdentity<M>(this->weights_);
|
H_ = kroneckerProductIdentity(M_, this->weights_);
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
@ -200,25 +191,26 @@ class Basis {
|
||||||
VectorEvaluationFunctor() {}
|
VectorEvaluationFunctor() {}
|
||||||
|
|
||||||
/// Default Constructor
|
/// 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();
|
calculateJacobian();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Constructor, with interval [a,b]
|
/// Constructor, with interval [a,b]
|
||||||
VectorEvaluationFunctor(size_t N, double x, double a, double b)
|
VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
|
||||||
: EvaluationFunctor(N, x, a, b) {
|
: EvaluationFunctor(N, x, a, b), M_(M) {
|
||||||
calculateJacobian();
|
calculateJacobian();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// M-dimensional evaluation
|
/// M-dimensional evaluation
|
||||||
VectorM apply(const ParameterMatrix<M>& P,
|
Vector apply(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
||||||
if (H) *H = H_;
|
if (H) *H = H_;
|
||||||
return P.matrix() * this->weights_.transpose();
|
return P.matrix() * this->weights_.transpose();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// c++ sugar
|
/// c++ sugar
|
||||||
VectorM operator()(const ParameterMatrix<M>& P,
|
Vector operator()(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
||||||
return apply(P, H);
|
return apply(P, H);
|
||||||
}
|
}
|
||||||
|
@ -231,13 +223,14 @@ class Basis {
|
||||||
*
|
*
|
||||||
* This component is specified by the row index i, with 0<i<M.
|
* This component is specified by the row index i, with 0<i<M.
|
||||||
*/
|
*/
|
||||||
template <int M>
|
|
||||||
class VectorComponentFunctor : public EvaluationFunctor {
|
class VectorComponentFunctor : public EvaluationFunctor {
|
||||||
protected:
|
protected:
|
||||||
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
|
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
|
||||||
size_t rowIndex_;
|
|
||||||
Jacobian H_;
|
Jacobian H_;
|
||||||
|
|
||||||
|
size_t M_;
|
||||||
|
size_t rowIndex_;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
|
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
|
||||||
* the M*N parameter matrix `P`.
|
* the M*N parameter matrix `P`.
|
||||||
|
@ -248,9 +241,9 @@ class Basis {
|
||||||
* MxM identity matrix. See also VectorEvaluationFunctor.
|
* MxM identity matrix. See also VectorEvaluationFunctor.
|
||||||
*/
|
*/
|
||||||
void calculateJacobian(size_t N) {
|
void calculateJacobian(size_t N) {
|
||||||
H_.setZero(1, M * N);
|
H_.setZero(1, M_ * N);
|
||||||
for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
|
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:
|
public:
|
||||||
|
@ -258,33 +251,34 @@ class Basis {
|
||||||
VectorComponentFunctor() {}
|
VectorComponentFunctor() {}
|
||||||
|
|
||||||
/// Construct with row index
|
/// Construct with row index
|
||||||
VectorComponentFunctor(size_t N, size_t i, double x)
|
VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
|
||||||
: EvaluationFunctor(N, x), rowIndex_(i) {
|
: EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
|
||||||
calculateJacobian(N);
|
calculateJacobian(N);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Construct with row index and interval
|
/// Construct with row index and interval
|
||||||
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
|
VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
|
||||||
: EvaluationFunctor(N, x, a, b), rowIndex_(i) {
|
double b)
|
||||||
|
: EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
|
||||||
calculateJacobian(N);
|
calculateJacobian(N);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Calculate component of component rowIndex_ of P
|
/// Calculate component of component rowIndex_ of P
|
||||||
double apply(const ParameterMatrix<M>& P,
|
double apply(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
||||||
if (H) *H = H_;
|
if (H) *H = H_;
|
||||||
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
|
return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// c++ sugar
|
/// c++ sugar
|
||||||
double operator()(const ParameterMatrix<M>& P,
|
double operator()(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
||||||
return apply(P, H);
|
return apply(P, H);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
|
* Manifold EvaluationFunctor at a given x, applied to ParameterMatrix.
|
||||||
* This functor is used to evaluate a parameterized function at a given scalar
|
* This functor is used to evaluate a parameterized function at a given scalar
|
||||||
* value x. When given a specific M*N parameters, returns an M-vector the M
|
* value x. When given a specific M*N parameters, returns an M-vector the M
|
||||||
* corresponding functions at x, possibly with Jacobians wrpt the parameters.
|
* corresponding functions at x, possibly with Jacobians wrpt the parameters.
|
||||||
|
@ -297,24 +291,23 @@ class Basis {
|
||||||
* 3D rotation.
|
* 3D rotation.
|
||||||
*/
|
*/
|
||||||
template <class T>
|
template <class T>
|
||||||
class ManifoldEvaluationFunctor
|
class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
|
||||||
: public VectorEvaluationFunctor<traits<T>::dimension> {
|
|
||||||
enum { M = traits<T>::dimension };
|
enum { M = traits<T>::dimension };
|
||||||
using Base = VectorEvaluationFunctor<M>;
|
using Base = VectorEvaluationFunctor;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// For serialization
|
/// For serialization
|
||||||
ManifoldEvaluationFunctor() {}
|
ManifoldEvaluationFunctor() {}
|
||||||
|
|
||||||
/// Default Constructor
|
/// 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]
|
/// Constructor, with interval [a,b]
|
||||||
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
|
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
|
||||||
: Base(N, x, a, b) {}
|
: Base(M, N, x, a, b) {}
|
||||||
|
|
||||||
/// Manifold evaluation
|
/// Manifold evaluation
|
||||||
T apply(const ParameterMatrix<M>& P,
|
T apply(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
||||||
// Interpolate the M-dimensional vector to yield a vector in tangent space
|
// Interpolate the M-dimensional vector to yield a vector in tangent space
|
||||||
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
|
Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
|
||||||
|
@ -333,7 +326,7 @@ class Basis {
|
||||||
}
|
}
|
||||||
|
|
||||||
/// c++ sugar
|
/// c++ sugar
|
||||||
T operator()(const ParameterMatrix<M>& P,
|
T operator()(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
|
||||||
return apply(P, H); // might call apply in derived
|
return apply(P, H); // might call apply in derived
|
||||||
}
|
}
|
||||||
|
@ -389,20 +382,20 @@ class Basis {
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
|
* VectorDerivativeFunctor at a given x, applied to ParameterMatrix.
|
||||||
*
|
*
|
||||||
* This functor is used to evaluate the derivatives of a parameterized
|
* This functor is used to evaluate the derivatives of a parameterized
|
||||||
* function at a given scalar value x. When given a specific M*N parameters,
|
* function at a given scalar value x. When given a specific M*N parameters,
|
||||||
* returns an M-vector the M corresponding function derivatives at x, possibly
|
* returns an M-vector the M corresponding function derivatives at x, possibly
|
||||||
* with Jacobians wrpt the parameters.
|
* with Jacobians wrpt the parameters.
|
||||||
*/
|
*/
|
||||||
template <int M>
|
|
||||||
class VectorDerivativeFunctor : protected DerivativeFunctorBase {
|
class VectorDerivativeFunctor : protected DerivativeFunctorBase {
|
||||||
protected:
|
protected:
|
||||||
using VectorM = Eigen::Matrix<double, M, 1>;
|
using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
|
||||||
using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
|
|
||||||
Jacobian H_;
|
Jacobian H_;
|
||||||
|
|
||||||
|
size_t M_;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
|
* Calculate the `M*(M*N)` Jacobian of this functor with respect to
|
||||||
* the M*N parameter matrix `P`.
|
* the M*N parameter matrix `P`.
|
||||||
|
@ -412,7 +405,7 @@ class Basis {
|
||||||
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
|
* i.e., the Kronecker product of weights_ with the MxM identity matrix.
|
||||||
*/
|
*/
|
||||||
void calculateJacobian() {
|
void calculateJacobian() {
|
||||||
H_ = kroneckerProductIdentity<M>(this->weights_);
|
H_ = kroneckerProductIdentity(M_, this->weights_);
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
@ -422,24 +415,24 @@ class Basis {
|
||||||
VectorDerivativeFunctor() {}
|
VectorDerivativeFunctor() {}
|
||||||
|
|
||||||
/// Default Constructor
|
/// 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();
|
calculateJacobian();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Constructor, with optional interval [a,b]
|
/// Constructor, with optional interval [a,b]
|
||||||
VectorDerivativeFunctor(size_t N, double x, double a, double b)
|
VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
|
||||||
: DerivativeFunctorBase(N, x, a, b) {
|
: DerivativeFunctorBase(N, x, a, b), M_(M) {
|
||||||
calculateJacobian();
|
calculateJacobian();
|
||||||
}
|
}
|
||||||
|
|
||||||
VectorM apply(const ParameterMatrix<M>& P,
|
Vector apply(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
||||||
if (H) *H = H_;
|
if (H) *H = H_;
|
||||||
return P.matrix() * this->weights_.transpose();
|
return P.matrix() * this->weights_.transpose();
|
||||||
}
|
}
|
||||||
/// c++ sugar
|
/// c++ sugar
|
||||||
VectorM operator()(
|
Vector operator()(const ParameterMatrix& P,
|
||||||
const ParameterMatrix<M>& P,
|
|
||||||
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*MxMN*/ -1, -1> H = {}) const {
|
||||||
return apply(P, H);
|
return apply(P, H);
|
||||||
}
|
}
|
||||||
|
@ -452,13 +445,14 @@ class Basis {
|
||||||
*
|
*
|
||||||
* This component is specified by the row index i, with 0<i<M.
|
* This component is specified by the row index i, with 0<i<M.
|
||||||
*/
|
*/
|
||||||
template <int M>
|
|
||||||
class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
|
class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
|
||||||
protected:
|
protected:
|
||||||
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
|
using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
|
||||||
size_t rowIndex_;
|
|
||||||
Jacobian H_;
|
Jacobian H_;
|
||||||
|
|
||||||
|
size_t M_;
|
||||||
|
size_t rowIndex_;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
|
* Calculate the `1*(M*N)` Jacobian of this functor with respect to
|
||||||
* the M*N parameter matrix `P`.
|
* the M*N parameter matrix `P`.
|
||||||
|
@ -469,9 +463,9 @@ class Basis {
|
||||||
* MxM identity matrix. See also VectorDerivativeFunctor.
|
* MxM identity matrix. See also VectorDerivativeFunctor.
|
||||||
*/
|
*/
|
||||||
void calculateJacobian(size_t N) {
|
void calculateJacobian(size_t N) {
|
||||||
H_.setZero(1, M * N);
|
H_.setZero(1, M_ * N);
|
||||||
for (int j = 0; j < this->weights_.size(); j++)
|
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:
|
public:
|
||||||
|
@ -479,29 +473,29 @@ class Basis {
|
||||||
ComponentDerivativeFunctor() {}
|
ComponentDerivativeFunctor() {}
|
||||||
|
|
||||||
/// Construct with row index
|
/// Construct with row index
|
||||||
ComponentDerivativeFunctor(size_t N, size_t i, double x)
|
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
|
||||||
: DerivativeFunctorBase(N, x), rowIndex_(i) {
|
: DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
|
||||||
calculateJacobian(N);
|
calculateJacobian(N);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Construct with row index and interval
|
/// Construct with row index and interval
|
||||||
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
|
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
|
||||||
: DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
|
double b)
|
||||||
|
: DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
|
||||||
calculateJacobian(N);
|
calculateJacobian(N);
|
||||||
}
|
}
|
||||||
/// Calculate derivative of component rowIndex_ of F
|
/// Calculate derivative of component rowIndex_ of F
|
||||||
double apply(const ParameterMatrix<M>& P,
|
double apply(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
||||||
if (H) *H = H_;
|
if (H) *H = H_;
|
||||||
return P.row(rowIndex_) * this->weights_.transpose();
|
return P.row(rowIndex_) * this->weights_.transpose();
|
||||||
}
|
}
|
||||||
/// c++ sugar
|
/// c++ sugar
|
||||||
double operator()(const ParameterMatrix<M>& P,
|
double operator()(const ParameterMatrix& P,
|
||||||
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
OptionalJacobian</*1xMN*/ -1, -1> H = {}) const {
|
||||||
return apply(P, H);
|
return apply(P, H);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
@ -87,15 +87,14 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> {
|
||||||
* measurement prediction function.
|
* measurement prediction function.
|
||||||
*
|
*
|
||||||
* @param BASIS: The basis class to use e.g. Chebyshev2
|
* @param BASIS: The basis class to use e.g. Chebyshev2
|
||||||
* @param M: Size of the evaluated state vector.
|
|
||||||
*
|
*
|
||||||
* @ingroup basis
|
* @ingroup basis
|
||||||
*/
|
*/
|
||||||
template <class BASIS, int M>
|
template <class BASIS>
|
||||||
class VectorEvaluationFactor
|
class VectorEvaluationFactor
|
||||||
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
|
: public FunctorizedFactor<Vector, ParameterMatrix> {
|
||||||
private:
|
private:
|
||||||
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
|
using Base = FunctorizedFactor<Vector, ParameterMatrix>;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
VectorEvaluationFactor() {}
|
VectorEvaluationFactor() {}
|
||||||
|
@ -107,14 +106,14 @@ class VectorEvaluationFactor
|
||||||
* polynomial.
|
* polynomial.
|
||||||
* @param z The measurement value.
|
* @param z The measurement value.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param M Size of the evaluated state vector.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
*/
|
*/
|
||||||
VectorEvaluationFactor(Key key, const Vector &z,
|
VectorEvaluationFactor(Key key, const Vector &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t M,
|
||||||
double x)
|
const size_t N, double x)
|
||||||
: Base(key, z, model,
|
: Base(key, z, model, typename BASIS::VectorEvaluationFunctor(M, N, x)) {}
|
||||||
typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Construct a new VectorEvaluationFactor object.
|
* @brief Construct a new VectorEvaluationFactor object.
|
||||||
|
@ -123,16 +122,17 @@ class VectorEvaluationFactor
|
||||||
* polynomial.
|
* polynomial.
|
||||||
* @param z The measurement value.
|
* @param z The measurement value.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param M Size of the evaluated state vector.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
* @param a Lower bound for the polynomial.
|
* @param a Lower bound for the polynomial.
|
||||||
* @param b Upper bound for the polynomial.
|
* @param b Upper bound for the polynomial.
|
||||||
*/
|
*/
|
||||||
VectorEvaluationFactor(Key key, const Vector &z,
|
VectorEvaluationFactor(Key key, const Vector &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t M,
|
||||||
double x, double a, double b)
|
const size_t N, double x, double a, double b)
|
||||||
: Base(key, z, model,
|
: Base(key, z, model,
|
||||||
typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
|
typename BASIS::VectorEvaluationFunctor(M, N, x, a, b)) {}
|
||||||
|
|
||||||
virtual ~VectorEvaluationFactor() {}
|
virtual ~VectorEvaluationFactor() {}
|
||||||
};
|
};
|
||||||
|
@ -147,20 +147,19 @@ class VectorEvaluationFactor
|
||||||
* indexed by `i`.
|
* indexed by `i`.
|
||||||
*
|
*
|
||||||
* @param BASIS: The basis class to use e.g. Chebyshev2
|
* @param BASIS: The basis class to use e.g. Chebyshev2
|
||||||
* @param P: Size of the fixed-size vector.
|
|
||||||
*
|
*
|
||||||
* Example:
|
* Example:
|
||||||
* VectorComponentFactor<BASIS, P> controlPrior(key, measured, model,
|
* VectorComponentFactor<BASIS> controlPrior(key, measured, model,
|
||||||
* N, i, t, a, b);
|
* N, i, t, a, b);
|
||||||
* where N is the degree and i is the component index.
|
* where N is the degree and i is the component index.
|
||||||
*
|
*
|
||||||
* @ingroup basis
|
* @ingroup basis
|
||||||
*/
|
*/
|
||||||
template <class BASIS, size_t P>
|
template <class BASIS>
|
||||||
class VectorComponentFactor
|
class VectorComponentFactor
|
||||||
: public FunctorizedFactor<double, ParameterMatrix<P>> {
|
: public FunctorizedFactor<double, ParameterMatrix> {
|
||||||
private:
|
private:
|
||||||
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
|
using Base = FunctorizedFactor<double, ParameterMatrix>;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
VectorComponentFactor() {}
|
VectorComponentFactor() {}
|
||||||
|
@ -173,15 +172,16 @@ class VectorComponentFactor
|
||||||
* @param z The scalar value at a specified index `i` of the full measurement
|
* @param z The scalar value at a specified index `i` of the full measurement
|
||||||
* vector.
|
* vector.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param P Size of the fixed-size vector.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param i The index for the evaluated vector to give us the desired scalar
|
* @param i The index for the evaluated vector to give us the desired scalar
|
||||||
* value.
|
* value.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
*/
|
*/
|
||||||
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
|
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,
|
: 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.
|
* @brief Construct a new VectorComponentFactor object.
|
||||||
|
@ -191,6 +191,7 @@ class VectorComponentFactor
|
||||||
* @param z The scalar value at a specified index `i` of the full measurement
|
* @param z The scalar value at a specified index `i` of the full measurement
|
||||||
* vector.
|
* vector.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param P Size of the fixed-size vector.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param i The index for the evaluated vector to give us the desired scalar
|
* @param i The index for the evaluated vector to give us the desired scalar
|
||||||
* value.
|
* value.
|
||||||
|
@ -199,11 +200,10 @@ class VectorComponentFactor
|
||||||
* @param b Upper bound for the polynomial.
|
* @param b Upper bound for the polynomial.
|
||||||
*/
|
*/
|
||||||
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
|
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
|
||||||
const size_t N, size_t i, double x, double a, double b)
|
const size_t P, const size_t N, size_t i, double x,
|
||||||
: Base(
|
double a, double b)
|
||||||
key, z, model,
|
: Base(key, z, model,
|
||||||
typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
|
typename BASIS::VectorComponentFunctor(P, N, i, x, a, b)) {}
|
||||||
}
|
|
||||||
|
|
||||||
virtual ~VectorComponentFactor() {}
|
virtual ~VectorComponentFactor() {}
|
||||||
};
|
};
|
||||||
|
@ -226,10 +226,9 @@ class VectorComponentFactor
|
||||||
* where `x` is the value (e.g. timestep) at which the rotation was evaluated.
|
* where `x` is the value (e.g. timestep) at which the rotation was evaluated.
|
||||||
*/
|
*/
|
||||||
template <class BASIS, typename T>
|
template <class BASIS, typename T>
|
||||||
class ManifoldEvaluationFactor
|
class ManifoldEvaluationFactor : public FunctorizedFactor<T, ParameterMatrix> {
|
||||||
: public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
|
|
||||||
private:
|
private:
|
||||||
using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
|
using Base = FunctorizedFactor<T, ParameterMatrix>;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ManifoldEvaluationFactor() {}
|
ManifoldEvaluationFactor() {}
|
||||||
|
@ -324,14 +323,13 @@ class DerivativeFactor
|
||||||
* polynomial at a specified point `x` is equal to the vector value `z`.
|
* polynomial at a specified point `x` is equal to the vector value `z`.
|
||||||
*
|
*
|
||||||
* @param BASIS: The basis class to use e.g. Chebyshev2
|
* @param BASIS: The basis class to use e.g. Chebyshev2
|
||||||
* @param M: Size of the evaluated state vector derivative.
|
|
||||||
*/
|
*/
|
||||||
template <class BASIS, int M>
|
template <class BASIS>
|
||||||
class VectorDerivativeFactor
|
class VectorDerivativeFactor
|
||||||
: public FunctorizedFactor<Vector, ParameterMatrix<M>> {
|
: public FunctorizedFactor<Vector, ParameterMatrix> {
|
||||||
private:
|
private:
|
||||||
using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
|
using Base = FunctorizedFactor<Vector, ParameterMatrix>;
|
||||||
using Func = typename BASIS::template VectorDerivativeFunctor<M>;
|
using Func = typename BASIS::VectorDerivativeFunctor;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
VectorDerivativeFactor() {}
|
VectorDerivativeFactor() {}
|
||||||
|
@ -343,13 +341,14 @@ class VectorDerivativeFactor
|
||||||
* polynomial.
|
* polynomial.
|
||||||
* @param z The measurement value.
|
* @param z The measurement value.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param M Size of the evaluated state vector derivative.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
*/
|
*/
|
||||||
VectorDerivativeFactor(Key key, const Vector &z,
|
VectorDerivativeFactor(Key key, const Vector &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t M,
|
||||||
double x)
|
const size_t N, double x)
|
||||||
: Base(key, z, model, Func(N, x)) {}
|
: Base(key, z, model, Func(M, N, x)) {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Construct a new VectorDerivativeFactor object.
|
* @brief Construct a new VectorDerivativeFactor object.
|
||||||
|
@ -358,15 +357,16 @@ class VectorDerivativeFactor
|
||||||
* polynomial.
|
* polynomial.
|
||||||
* @param z The measurement value.
|
* @param z The measurement value.
|
||||||
* @param model The noise model.
|
* @param model The noise model.
|
||||||
|
* @param M Size of the evaluated state vector derivative.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
* @param a Lower bound for the polynomial.
|
* @param a Lower bound for the polynomial.
|
||||||
* @param b Upper bound for the polynomial.
|
* @param b Upper bound for the polynomial.
|
||||||
*/
|
*/
|
||||||
VectorDerivativeFactor(Key key, const Vector &z,
|
VectorDerivativeFactor(Key key, const Vector &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t M,
|
||||||
double x, double a, double b)
|
const size_t N, double x, double a, double b)
|
||||||
: Base(key, z, model, Func(N, x, a, b)) {}
|
: Base(key, z, model, Func(M, N, x, a, b)) {}
|
||||||
|
|
||||||
virtual ~VectorDerivativeFactor() {}
|
virtual ~VectorDerivativeFactor() {}
|
||||||
};
|
};
|
||||||
|
@ -377,14 +377,13 @@ class VectorDerivativeFactor
|
||||||
* vector-valued measurement `z`.
|
* vector-valued measurement `z`.
|
||||||
*
|
*
|
||||||
* @param BASIS: The basis class to use e.g. Chebyshev2
|
* @param BASIS: The basis class to use e.g. Chebyshev2
|
||||||
* @param P: Size of the control component derivative.
|
|
||||||
*/
|
*/
|
||||||
template <class BASIS, int P>
|
template <class BASIS>
|
||||||
class ComponentDerivativeFactor
|
class ComponentDerivativeFactor
|
||||||
: public FunctorizedFactor<double, ParameterMatrix<P>> {
|
: public FunctorizedFactor<double, ParameterMatrix> {
|
||||||
private:
|
private:
|
||||||
using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
|
using Base = FunctorizedFactor<double, ParameterMatrix>;
|
||||||
using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
|
using Func = typename BASIS::ComponentDerivativeFunctor;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ComponentDerivativeFactor() {}
|
ComponentDerivativeFactor() {}
|
||||||
|
@ -397,15 +396,16 @@ class ComponentDerivativeFactor
|
||||||
* @param z The scalar measurement value at a specific index `i` of the full
|
* @param z The scalar measurement value at a specific index `i` of the full
|
||||||
* measurement vector.
|
* measurement vector.
|
||||||
* @param model The degree of the polynomial.
|
* @param model The degree of the polynomial.
|
||||||
|
* @param P: Size of the control component derivative.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param i The index for the evaluated vector to give us the desired scalar
|
* @param i The index for the evaluated vector to give us the desired scalar
|
||||||
* value.
|
* value.
|
||||||
* @param x The point at which to evaluate the basis polynomial.
|
* @param x The point at which to evaluate the basis polynomial.
|
||||||
*/
|
*/
|
||||||
ComponentDerivativeFactor(Key key, const double &z,
|
ComponentDerivativeFactor(Key key, const double &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t P,
|
||||||
size_t i, double x)
|
const size_t N, size_t i, double x)
|
||||||
: Base(key, z, model, Func(N, i, x)) {}
|
: Base(key, z, model, Func(P, N, i, x)) {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Construct a new ComponentDerivativeFactor object.
|
* @brief Construct a new ComponentDerivativeFactor object.
|
||||||
|
@ -415,6 +415,7 @@ class ComponentDerivativeFactor
|
||||||
* @param z The scalar measurement value at a specific index `i` of the full
|
* @param z The scalar measurement value at a specific index `i` of the full
|
||||||
* measurement vector.
|
* measurement vector.
|
||||||
* @param model The degree of the polynomial.
|
* @param model The degree of the polynomial.
|
||||||
|
* @param P: Size of the control component derivative.
|
||||||
* @param N The degree of the polynomial.
|
* @param N The degree of the polynomial.
|
||||||
* @param i The index for the evaluated vector to give us the desired scalar
|
* @param i The index for the evaluated vector to give us the desired scalar
|
||||||
* value.
|
* value.
|
||||||
|
@ -423,9 +424,10 @@ class ComponentDerivativeFactor
|
||||||
* @param b Upper bound for the polynomial.
|
* @param b Upper bound for the polynomial.
|
||||||
*/
|
*/
|
||||||
ComponentDerivativeFactor(Key key, const double &z,
|
ComponentDerivativeFactor(Key key, const double &z,
|
||||||
const SharedNoiseModel &model, const size_t N,
|
const SharedNoiseModel &model, const size_t P,
|
||||||
size_t i, double x, double a, double b)
|
const size_t N, size_t i, double x, double a,
|
||||||
: Base(key, z, model, Func(N, i, x, a, b)) {}
|
double b)
|
||||||
|
: Base(key, z, model, Func(P, N, i, x, a, b)) {}
|
||||||
|
|
||||||
virtual ~ComponentDerivativeFactor() {}
|
virtual ~ComponentDerivativeFactor() {}
|
||||||
};
|
};
|
||||||
|
|
|
@ -30,16 +30,12 @@ namespace gtsam {
|
||||||
/**
|
/**
|
||||||
* A matrix abstraction of MxN values at the Basis points.
|
* A matrix abstraction of MxN values at the Basis points.
|
||||||
* This class serves as a wrapper over an Eigen matrix.
|
* This class serves as a wrapper over an Eigen matrix.
|
||||||
* @tparam M: The dimension of the type you wish to evaluate.
|
|
||||||
* @param N: the number of Basis points (e.g. Chebyshev points of the second
|
* @param N: the number of Basis points (e.g. Chebyshev points of the second
|
||||||
* kind).
|
* kind).
|
||||||
*/
|
*/
|
||||||
template <int M>
|
|
||||||
class ParameterMatrix {
|
class ParameterMatrix {
|
||||||
using MatrixType = Eigen::Matrix<double, M, -1>;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
MatrixType matrix_;
|
Matrix matrix_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||||
|
@ -48,15 +44,18 @@ class ParameterMatrix {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create ParameterMatrix using the number of basis points.
|
* Create ParameterMatrix using the number of basis points.
|
||||||
|
* @param M: The dimension size of the type you wish to evaluate.
|
||||||
* @param N: The number of basis points (the columns).
|
* @param N: The number of basis points (the columns).
|
||||||
*/
|
*/
|
||||||
ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
|
ParameterMatrix(const size_t M, const size_t N) : matrix_(M, N) {
|
||||||
|
matrix_.setZero();
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create ParameterMatrix from an MxN Eigen Matrix.
|
* Create ParameterMatrix from an MxN Eigen Matrix.
|
||||||
* @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
|
* @param matrix: An Eigen matrix used to initialze the ParameterMatrix.
|
||||||
*/
|
*/
|
||||||
ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
|
ParameterMatrix(const Matrix& matrix) : matrix_(matrix) {}
|
||||||
|
|
||||||
/// Get the number of rows.
|
/// Get the number of rows.
|
||||||
size_t rows() const { return matrix_.rows(); }
|
size_t rows() const { return matrix_.rows(); }
|
||||||
|
@ -65,10 +64,10 @@ class ParameterMatrix {
|
||||||
size_t cols() const { return matrix_.cols(); }
|
size_t cols() const { return matrix_.cols(); }
|
||||||
|
|
||||||
/// Get the underlying matrix.
|
/// Get the underlying matrix.
|
||||||
MatrixType matrix() const { return matrix_; }
|
Matrix matrix() const { return matrix_; }
|
||||||
|
|
||||||
/// Return the tranpose of the underlying matrix.
|
/// Return the tranpose of the underlying matrix.
|
||||||
Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
|
Matrix transpose() const { return matrix_.transpose(); }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the matrix row specified by `index`.
|
* Get the matrix row specified by `index`.
|
||||||
|
@ -82,7 +81,7 @@ class ParameterMatrix {
|
||||||
* Set the matrix row specified by `index`.
|
* Set the matrix row specified by `index`.
|
||||||
* @param index: The row index to set.
|
* @param index: The row index to set.
|
||||||
*/
|
*/
|
||||||
auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
|
auto row(size_t index) -> Eigen::Block<Matrix, 1, -1, false> {
|
||||||
return matrix_.row(index);
|
return matrix_.row(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -90,7 +89,7 @@ class ParameterMatrix {
|
||||||
* Get the matrix column specified by `index`.
|
* Get the matrix column specified by `index`.
|
||||||
* @param index: The column index to retrieve.
|
* @param index: The column index to retrieve.
|
||||||
*/
|
*/
|
||||||
Eigen::Matrix<double, M, 1> col(size_t index) const {
|
Eigen::Matrix<double, -1, 1> col(size_t index) const {
|
||||||
return matrix_.col(index);
|
return matrix_.col(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -98,7 +97,7 @@ class ParameterMatrix {
|
||||||
* Set the matrix column specified by `index`.
|
* Set the matrix column specified by `index`.
|
||||||
* @param index: The column index to set.
|
* @param index: The column index to set.
|
||||||
*/
|
*/
|
||||||
auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
|
auto col(size_t index) -> Eigen::Block<Matrix, -1, 1, true> {
|
||||||
return matrix_.col(index);
|
return matrix_.col(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -111,37 +110,35 @@ class ParameterMatrix {
|
||||||
* Add a ParameterMatrix to another.
|
* Add a ParameterMatrix to another.
|
||||||
* @param other: ParameterMatrix to add.
|
* @param other: ParameterMatrix to add.
|
||||||
*/
|
*/
|
||||||
ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
|
ParameterMatrix operator+(const ParameterMatrix& other) const {
|
||||||
return ParameterMatrix<M>(matrix_ + other.matrix());
|
return ParameterMatrix(matrix_ + other.matrix());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a MxN-sized vector to the ParameterMatrix.
|
* Add a MxN-sized vector to the ParameterMatrix.
|
||||||
* @param other: Vector which is reshaped and added.
|
* @param other: Vector which is reshaped and added.
|
||||||
*/
|
*/
|
||||||
ParameterMatrix<M> operator+(
|
ParameterMatrix operator+(const Eigen::Matrix<double, -1, 1>& other) const {
|
||||||
const Eigen::Matrix<double, -1, 1>& other) const {
|
|
||||||
// This form avoids a deep copy and instead typecasts `other`.
|
// This form avoids a deep copy and instead typecasts `other`.
|
||||||
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
|
Eigen::Map<const Matrix> other_(other.data(), rows(), cols());
|
||||||
return ParameterMatrix<M>(matrix_ + other_);
|
return ParameterMatrix(matrix_ + other_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Subtract a ParameterMatrix from another.
|
* Subtract a ParameterMatrix from another.
|
||||||
* @param other: ParameterMatrix to subtract.
|
* @param other: ParameterMatrix to subtract.
|
||||||
*/
|
*/
|
||||||
ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
|
ParameterMatrix operator-(const ParameterMatrix& other) const {
|
||||||
return ParameterMatrix<M>(matrix_ - other.matrix());
|
return ParameterMatrix(matrix_ - other.matrix());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Subtract a MxN-sized vector from the ParameterMatrix.
|
* Subtract a MxN-sized vector from the ParameterMatrix.
|
||||||
* @param other: Vector which is reshaped and subracted.
|
* @param other: Vector which is reshaped and subracted.
|
||||||
*/
|
*/
|
||||||
ParameterMatrix<M> operator-(
|
ParameterMatrix operator-(const Eigen::Matrix<double, -1, 1>& other) const {
|
||||||
const Eigen::Matrix<double, -1, 1>& other) const {
|
Eigen::Map<const Matrix> other_(other.data(), rows(), cols());
|
||||||
Eigen::Map<const MatrixType> other_(other.data(), M, cols());
|
return ParameterMatrix(matrix_ - other_);
|
||||||
return ParameterMatrix<M>(matrix_ - other_);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -149,9 +146,7 @@ class ParameterMatrix {
|
||||||
* @param other: Eigen matrix which should be multiplication compatible with
|
* @param other: Eigen matrix which should be multiplication compatible with
|
||||||
* the ParameterMatrix.
|
* the ParameterMatrix.
|
||||||
*/
|
*/
|
||||||
MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
|
Matrix operator*(const Matrix& other) const { return matrix_ * other; }
|
||||||
return matrix_ * other;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @name Vector Space requirements
|
/// @name Vector Space requirements
|
||||||
/// @{
|
/// @{
|
||||||
|
@ -169,7 +164,7 @@ class ParameterMatrix {
|
||||||
* @param other: The ParameterMatrix to check equality with.
|
* @param other: The ParameterMatrix to check equality with.
|
||||||
* @param tol: The absolute tolerance threshold.
|
* @param tol: The absolute tolerance threshold.
|
||||||
*/
|
*/
|
||||||
bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
|
bool equals(const ParameterMatrix& other, double tol = 1e-8) const {
|
||||||
return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
|
return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -189,25 +184,22 @@ class ParameterMatrix {
|
||||||
* NOTE: The size at compile time is unknown so this identity is zero
|
* NOTE: The size at compile time is unknown so this identity is zero
|
||||||
* length and thus not valid.
|
* length and thus not valid.
|
||||||
*/
|
*/
|
||||||
inline static ParameterMatrix Identity() {
|
inline static ParameterMatrix Identity(size_t M = 0, size_t N = 0) {
|
||||||
// throw std::runtime_error(
|
return ParameterMatrix(M, N);
|
||||||
// "ParameterMatrix::Identity(): Don't use this function");
|
|
||||||
return ParameterMatrix(0);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
};
|
};
|
||||||
|
|
||||||
// traits for ParameterMatrix
|
/// traits for ParameterMatrix
|
||||||
template <int M>
|
template <>
|
||||||
struct traits<ParameterMatrix<M>>
|
struct traits<ParameterMatrix> : public internal::VectorSpace<ParameterMatrix> {
|
||||||
: public internal::VectorSpace<ParameterMatrix<M>> {};
|
};
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Stream operator that takes a ParameterMatrix. Used for printing.
|
// Stream operator that takes a ParameterMatrix. Used for printing.
|
||||||
template <int M>
|
|
||||||
inline std::ostream& operator<<(std::ostream& os,
|
inline std::ostream& operator<<(std::ostream& os,
|
||||||
const ParameterMatrix<M>& parameterMatrix) {
|
const ParameterMatrix& parameterMatrix) {
|
||||||
os << parameterMatrix.matrix();
|
os << parameterMatrix.matrix();
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
|
@ -48,9 +48,8 @@ class Chebyshev2 {
|
||||||
|
|
||||||
#include <gtsam/basis/ParameterMatrix.h>
|
#include <gtsam/basis/ParameterMatrix.h>
|
||||||
|
|
||||||
template <M = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}>
|
|
||||||
class ParameterMatrix {
|
class ParameterMatrix {
|
||||||
ParameterMatrix(const size_t N);
|
ParameterMatrix(const size_t M, const size_t N);
|
||||||
ParameterMatrix(const Matrix& matrix);
|
ParameterMatrix(const Matrix& matrix);
|
||||||
|
|
||||||
Matrix matrix() const;
|
Matrix matrix() const;
|
||||||
|
@ -72,45 +71,36 @@ virtual class EvaluationFactor : gtsam::NoiseModelFactor {
|
||||||
double x, double a, double b);
|
double x, double a, double b);
|
||||||
};
|
};
|
||||||
|
|
||||||
template <BASIS, M>
|
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
||||||
|
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
|
||||||
virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
|
virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor {
|
||||||
VectorEvaluationFactor();
|
VectorEvaluationFactor();
|
||||||
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
|
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
|
||||||
const gtsam::noiseModel::Base* model, const size_t N,
|
const gtsam::noiseModel::Base* model, const size_t M,
|
||||||
double x);
|
const size_t N, double x);
|
||||||
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
|
VectorEvaluationFactor(gtsam::Key key, const Vector& z,
|
||||||
const gtsam::noiseModel::Base* model, const size_t N,
|
const gtsam::noiseModel::Base* model, const size_t M,
|
||||||
double x, double a, double b);
|
const size_t N, double x, double a, double b);
|
||||||
};
|
};
|
||||||
|
|
||||||
// TODO(Varun) Better way to support arbitrary dimensions?
|
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
||||||
// Especially if users mainly do `pip install gtsam` for the Python wrapper.
|
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
|
||||||
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 3>
|
|
||||||
VectorEvaluationFactorChebyshev2D3;
|
|
||||||
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 4>
|
|
||||||
VectorEvaluationFactorChebyshev2D4;
|
|
||||||
typedef gtsam::VectorEvaluationFactor<gtsam::Chebyshev2, 12>
|
|
||||||
VectorEvaluationFactorChebyshev2D12;
|
|
||||||
|
|
||||||
template <BASIS, P>
|
|
||||||
virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
|
virtual class VectorComponentFactor : gtsam::NoiseModelFactor {
|
||||||
VectorComponentFactor();
|
VectorComponentFactor();
|
||||||
VectorComponentFactor(gtsam::Key key, const double z,
|
VectorComponentFactor(gtsam::Key key, const double z,
|
||||||
const gtsam::noiseModel::Base* model, const size_t N,
|
const gtsam::noiseModel::Base* model, const size_t M,
|
||||||
size_t i, double x);
|
const size_t N, size_t i, double x);
|
||||||
VectorComponentFactor(gtsam::Key key, const double z,
|
VectorComponentFactor(gtsam::Key key, const double z,
|
||||||
const gtsam::noiseModel::Base* model, const size_t N,
|
const gtsam::noiseModel::Base* model, const size_t M,
|
||||||
size_t i, double x, double a, double b);
|
const size_t N, size_t i, double x, double a, double b);
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 3>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
VectorComponentFactorChebyshev2D3;
|
#include <gtsam/geometry/Pose3.h>
|
||||||
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 4>
|
|
||||||
VectorComponentFactorChebyshev2D4;
|
|
||||||
typedef gtsam::VectorComponentFactor<gtsam::Chebyshev2, 12>
|
|
||||||
VectorComponentFactorChebyshev2D12;
|
|
||||||
|
|
||||||
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 {
|
virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
|
||||||
ManifoldEvaluationFactor();
|
ManifoldEvaluationFactor();
|
||||||
ManifoldEvaluationFactor(gtsam::Key key, const T& z,
|
ManifoldEvaluationFactor(gtsam::Key key, const T& z,
|
||||||
|
@ -121,15 +111,42 @@ virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor {
|
||||||
double x, double a, double b);
|
double x, double a, double b);
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose3.h>
|
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);
|
||||||
|
};
|
||||||
|
|
||||||
typedef gtsam::ManifoldEvaluationFactor<gtsam::Chebyshev2, gtsam::Rot3>
|
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
||||||
ManifoldEvaluationFactorChebyshev2Rot3;
|
gtsam::Chebyshev2Basis, gtsam::Chebyshev2}>
|
||||||
typedef gtsam::ManifoldEvaluationFactor<gtsam::Chebyshev2, gtsam::Pose3>
|
virtual class VectorDerivativeFactor : gtsam::NoiseModelFactor {
|
||||||
ManifoldEvaluationFactorChebyshev2Pose3;
|
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);
|
||||||
|
};
|
||||||
|
|
||||||
// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and
|
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
||||||
// `ComponentDerivativeFactor`
|
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>
|
#include <gtsam/basis/FitBasis.h>
|
||||||
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
template <BASIS = {gtsam::FourierBasis, gtsam::Chebyshev1Basis,
|
||||||
|
|
|
@ -17,30 +17,29 @@
|
||||||
* @brief unit tests for factors in BasisFactors.h
|
* @brief unit tests for factors in BasisFactors.h
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
|
#include <gtsam/base/TestableAssertions.h>
|
||||||
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/basis/Basis.h>
|
#include <gtsam/basis/Basis.h>
|
||||||
#include <gtsam/basis/BasisFactors.h>
|
#include <gtsam/basis/BasisFactors.h>
|
||||||
#include <gtsam/basis/Chebyshev2.h>
|
#include <gtsam/basis/Chebyshev2.h>
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
#include <gtsam/nonlinear/FunctorizedFactor.h>
|
#include <gtsam/nonlinear/FunctorizedFactor.h>
|
||||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||||
#include <gtsam/nonlinear/factorTesting.h>
|
#include <gtsam/nonlinear/factorTesting.h>
|
||||||
#include <gtsam/inference/Symbol.h>
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/TestableAssertions.h>
|
|
||||||
#include <gtsam/base/Vector.h>
|
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
|
||||||
|
|
||||||
using gtsam::noiseModel::Isotropic;
|
|
||||||
using gtsam::Pose2;
|
|
||||||
using gtsam::Vector;
|
|
||||||
using gtsam::Values;
|
|
||||||
using gtsam::Chebyshev2;
|
using gtsam::Chebyshev2;
|
||||||
using gtsam::ParameterMatrix;
|
|
||||||
using gtsam::LevenbergMarquardtParams;
|
|
||||||
using gtsam::LevenbergMarquardtOptimizer;
|
using gtsam::LevenbergMarquardtOptimizer;
|
||||||
|
using gtsam::LevenbergMarquardtParams;
|
||||||
using gtsam::NonlinearFactorGraph;
|
using gtsam::NonlinearFactorGraph;
|
||||||
using gtsam::NonlinearOptimizerParams;
|
using gtsam::NonlinearOptimizerParams;
|
||||||
|
using gtsam::ParameterMatrix;
|
||||||
|
using gtsam::Pose2;
|
||||||
|
using gtsam::Values;
|
||||||
|
using gtsam::Vector;
|
||||||
|
using gtsam::noiseModel::Isotropic;
|
||||||
|
|
||||||
constexpr size_t N = 2;
|
constexpr size_t N = 2;
|
||||||
|
|
||||||
|
@ -81,15 +80,15 @@ TEST(BasisFactors, VectorEvaluationFactor) {
|
||||||
const Vector measured = Vector::Zero(M);
|
const Vector measured = Vector::Zero(M);
|
||||||
|
|
||||||
auto model = Isotropic::Sigma(M, 1.0);
|
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;
|
NonlinearFactorGraph graph;
|
||||||
graph.add(factor);
|
graph.add(factor);
|
||||||
|
|
||||||
ParameterMatrix<M> stateMatrix(N);
|
ParameterMatrix stateMatrix(M, N);
|
||||||
|
|
||||||
Values initial;
|
Values initial;
|
||||||
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
|
initial.insert<ParameterMatrix>(key, stateMatrix);
|
||||||
|
|
||||||
LevenbergMarquardtParams parameters;
|
LevenbergMarquardtParams parameters;
|
||||||
parameters.setMaxIterations(20);
|
parameters.setMaxIterations(20);
|
||||||
|
@ -107,7 +106,7 @@ TEST(BasisFactors, Print) {
|
||||||
const Vector measured = Vector::Ones(M) * 42;
|
const Vector measured = Vector::Ones(M) * 42;
|
||||||
|
|
||||||
auto model = Isotropic::Sigma(M, 1.0);
|
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 =
|
std::string expected =
|
||||||
" keys = { X0 }\n"
|
" keys = { X0 }\n"
|
||||||
|
@ -128,16 +127,16 @@ TEST(BasisFactors, VectorComponentFactor) {
|
||||||
const size_t i = 2;
|
const size_t i = 2;
|
||||||
const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
|
const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
|
||||||
auto model = Isotropic::Sigma(1, 1.0);
|
auto model = Isotropic::Sigma(1, 1.0);
|
||||||
VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i,
|
VectorComponentFactor<Chebyshev2> factor(key, measured, model, P, N, i, t, a,
|
||||||
t, a, b);
|
b);
|
||||||
|
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
graph.add(factor);
|
graph.add(factor);
|
||||||
|
|
||||||
ParameterMatrix<P> stateMatrix(N);
|
ParameterMatrix stateMatrix(P, N);
|
||||||
|
|
||||||
Values initial;
|
Values initial;
|
||||||
initial.insert<ParameterMatrix<P>>(key, stateMatrix);
|
initial.insert<ParameterMatrix>(key, stateMatrix);
|
||||||
|
|
||||||
LevenbergMarquardtParams parameters;
|
LevenbergMarquardtParams parameters;
|
||||||
parameters.setMaxIterations(20);
|
parameters.setMaxIterations(20);
|
||||||
|
@ -153,16 +152,16 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
|
||||||
const Pose2 measured;
|
const Pose2 measured;
|
||||||
const double t = 3.0, a = 2.0, b = 4.0;
|
const double t = 3.0, a = 2.0, b = 4.0;
|
||||||
auto model = Isotropic::Sigma(3, 1.0);
|
auto model = Isotropic::Sigma(3, 1.0);
|
||||||
ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N,
|
ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N, t,
|
||||||
t, a, b);
|
a, b);
|
||||||
|
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
graph.add(factor);
|
graph.add(factor);
|
||||||
|
|
||||||
ParameterMatrix<3> stateMatrix(N);
|
ParameterMatrix stateMatrix(3, N);
|
||||||
|
|
||||||
Values initial;
|
Values initial;
|
||||||
initial.insert<ParameterMatrix<3>>(key, stateMatrix);
|
initial.insert<ParameterMatrix>(key, stateMatrix);
|
||||||
|
|
||||||
LevenbergMarquardtParams parameters;
|
LevenbergMarquardtParams parameters;
|
||||||
parameters.setMaxIterations(20);
|
parameters.setMaxIterations(20);
|
||||||
|
@ -181,15 +180,15 @@ TEST(BasisFactors, VecDerivativePrior) {
|
||||||
|
|
||||||
const Vector measured = Vector::Zero(M);
|
const Vector measured = Vector::Zero(M);
|
||||||
auto model = Isotropic::Sigma(M, 1.0);
|
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;
|
NonlinearFactorGraph graph;
|
||||||
graph.add(vecDPrior);
|
graph.add(vecDPrior);
|
||||||
|
|
||||||
ParameterMatrix<M> stateMatrix(N);
|
ParameterMatrix stateMatrix(M, N);
|
||||||
|
|
||||||
Values initial;
|
Values initial;
|
||||||
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
|
initial.insert<ParameterMatrix>(key, stateMatrix);
|
||||||
|
|
||||||
LevenbergMarquardtParams parameters;
|
LevenbergMarquardtParams parameters;
|
||||||
parameters.setMaxIterations(20);
|
parameters.setMaxIterations(20);
|
||||||
|
@ -206,15 +205,15 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
|
||||||
|
|
||||||
double measured = 0;
|
double measured = 0;
|
||||||
auto model = Isotropic::Sigma(1, 1.0);
|
auto model = Isotropic::Sigma(1, 1.0);
|
||||||
ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model,
|
ComponentDerivativeFactor<Chebyshev2> controlDPrior(key, measured, model, M,
|
||||||
N, 0, 0);
|
N, 0, 0);
|
||||||
|
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
graph.add(controlDPrior);
|
graph.add(controlDPrior);
|
||||||
|
|
||||||
Values initial;
|
Values initial;
|
||||||
ParameterMatrix<M> stateMatrix(N);
|
ParameterMatrix stateMatrix(M, N);
|
||||||
initial.insert<ParameterMatrix<M>>(key, stateMatrix);
|
initial.insert<ParameterMatrix>(key, stateMatrix);
|
||||||
|
|
||||||
LevenbergMarquardtParams parameters;
|
LevenbergMarquardtParams parameters;
|
||||||
parameters.setMaxIterations(20);
|
parameters.setMaxIterations(20);
|
||||||
|
|
|
@ -108,7 +108,7 @@ TEST(Chebyshev2, InterpolateVector) {
|
||||||
double t = 30, a = 0, b = 100;
|
double t = 30, a = 0, b = 100;
|
||||||
const size_t N = 3;
|
const size_t N = 3;
|
||||||
// Create 2x3 matrix with Vectors at Chebyshev points
|
// Create 2x3 matrix with Vectors at Chebyshev points
|
||||||
ParameterMatrix<2> X(N);
|
ParameterMatrix X(2, N);
|
||||||
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
|
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
|
||||||
|
|
||||||
// Check value
|
// Check value
|
||||||
|
@ -116,15 +116,15 @@ TEST(Chebyshev2, InterpolateVector) {
|
||||||
expected << t, 0;
|
expected << t, 0;
|
||||||
Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N);
|
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));
|
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
|
||||||
|
|
||||||
// Check derivative
|
// Check derivative
|
||||||
std::function<Vector2(ParameterMatrix<2>)> f =
|
std::function<Vector2(ParameterMatrix)> f =
|
||||||
std::bind(&Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx,
|
std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
|
||||||
std::placeholders::_1, nullptr);
|
std::placeholders::_1, nullptr);
|
||||||
Matrix numericalH =
|
Matrix numericalH =
|
||||||
numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
|
numericalDerivative11<Vector2, ParameterMatrix, 2 * N>(f, X);
|
||||||
EXPECT(assert_equal(numericalH, actualH, 1e-9));
|
EXPECT(assert_equal(numericalH, actualH, 1e-9));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -133,7 +133,7 @@ TEST(Chebyshev2, InterpolateVector) {
|
||||||
TEST(Chebyshev2, InterpolatePose2) {
|
TEST(Chebyshev2, InterpolatePose2) {
|
||||||
double t = 30, a = 0, b = 100;
|
double t = 30, a = 0, b = 100;
|
||||||
|
|
||||||
ParameterMatrix<3> X(N);
|
ParameterMatrix X(3, N);
|
||||||
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
|
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
|
||||||
X.row(1) = Vector::Zero(N);
|
X.row(1) = Vector::Zero(N);
|
||||||
X.row(2) = 0.1 * Vector::Ones(N);
|
X.row(2) = 0.1 * Vector::Ones(N);
|
||||||
|
@ -148,11 +148,11 @@ TEST(Chebyshev2, InterpolatePose2) {
|
||||||
EXPECT(assert_equal(expected, fx(X, actualH)));
|
EXPECT(assert_equal(expected, fx(X, actualH)));
|
||||||
|
|
||||||
// Check derivative
|
// Check derivative
|
||||||
std::function<Pose2(ParameterMatrix<3>)> f =
|
std::function<Pose2(ParameterMatrix)> f =
|
||||||
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
|
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
|
||||||
std::placeholders::_1, nullptr);
|
std::placeholders::_1, nullptr);
|
||||||
Matrix numericalH =
|
Matrix numericalH =
|
||||||
numericalDerivative11<Pose2, ParameterMatrix<3>, 3 * N>(f, X);
|
numericalDerivative11<Pose2, ParameterMatrix, 3 * N>(f, X);
|
||||||
EXPECT(assert_equal(numericalH, actualH, 1e-9));
|
EXPECT(assert_equal(numericalH, actualH, 1e-9));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -169,7 +169,7 @@ TEST(Chebyshev2, InterpolatePose3) {
|
||||||
Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
|
Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
|
||||||
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
|
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N);
|
||||||
|
|
||||||
ParameterMatrix<6> X(N);
|
ParameterMatrix X(6, N);
|
||||||
X.col(11) = xi;
|
X.col(11) = xi;
|
||||||
|
|
||||||
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
|
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
|
||||||
|
@ -178,11 +178,11 @@ TEST(Chebyshev2, InterpolatePose3) {
|
||||||
EXPECT(assert_equal(expected, fx(X, actualH)));
|
EXPECT(assert_equal(expected, fx(X, actualH)));
|
||||||
|
|
||||||
// Check derivative
|
// Check derivative
|
||||||
std::function<Pose3(ParameterMatrix<6>)> f =
|
std::function<Pose3(ParameterMatrix)> f =
|
||||||
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
|
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
|
||||||
std::placeholders::_1, nullptr);
|
std::placeholders::_1, nullptr);
|
||||||
Matrix numericalH =
|
Matrix numericalH =
|
||||||
numericalDerivative11<Pose3, ParameterMatrix<6>, 6 * N>(f, X);
|
numericalDerivative11<Pose3, ParameterMatrix, 6 * N>(f, X);
|
||||||
EXPECT(assert_equal(numericalH, actualH, 1e-8));
|
EXPECT(assert_equal(numericalH, actualH, 1e-8));
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -413,14 +413,14 @@ TEST(Chebyshev2, Derivative6_03) {
|
||||||
TEST(Chebyshev2, VectorDerivativeFunctor) {
|
TEST(Chebyshev2, VectorDerivativeFunctor) {
|
||||||
const size_t N = 3, M = 2;
|
const size_t N = 3, M = 2;
|
||||||
const double x = 0.2;
|
const double x = 0.2;
|
||||||
using VecD = Chebyshev2::VectorDerivativeFunctor<M>;
|
using VecD = Chebyshev2::VectorDerivativeFunctor;
|
||||||
VecD fx(N, x, 0, 3);
|
VecD fx(M, N, x, 0, 3);
|
||||||
ParameterMatrix<M> X(N);
|
ParameterMatrix X(M, N);
|
||||||
Matrix actualH(M, M * N);
|
Matrix actualH(M, M * N);
|
||||||
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
|
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
|
||||||
|
|
||||||
// Test Jacobian
|
// Test Jacobian
|
||||||
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix<M>, M * N>(
|
Matrix expectedH = numericalDerivative11<Vector2, ParameterMatrix, M * N>(
|
||||||
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
|
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
|
||||||
EXPECT(assert_equal(expectedH, actualH, 1e-7));
|
EXPECT(assert_equal(expectedH, actualH, 1e-7));
|
||||||
}
|
}
|
||||||
|
@ -429,30 +429,30 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
|
||||||
// Test VectorDerivativeFunctor with polynomial function
|
// Test VectorDerivativeFunctor with polynomial function
|
||||||
TEST(Chebyshev2, VectorDerivativeFunctor2) {
|
TEST(Chebyshev2, VectorDerivativeFunctor2) {
|
||||||
const size_t N = 64, M = 1, T = 15;
|
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);
|
const Vector points = Chebyshev2::Points(N, 0, T);
|
||||||
|
|
||||||
// Assign the parameter matrix
|
// Assign the parameter matrix 1xN
|
||||||
Vector values(N);
|
Matrix values(1, N);
|
||||||
for (size_t i = 0; i < N; ++i) {
|
for (size_t i = 0; i < N; ++i) {
|
||||||
values(i) = f(points(i));
|
values(i) = f(points(i));
|
||||||
}
|
}
|
||||||
ParameterMatrix<M> X(values);
|
ParameterMatrix X(values);
|
||||||
|
|
||||||
// Evaluate the derivative at the chebyshev points using
|
// Evaluate the derivative at the chebyshev points using
|
||||||
// VectorDerivativeFunctor.
|
// VectorDerivativeFunctor.
|
||||||
for (size_t i = 0; i < N; ++i) {
|
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);
|
Vector1 Dx = d(X);
|
||||||
EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
|
EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test Jacobian at the first chebyshev point.
|
// Test Jacobian at the first chebyshev point.
|
||||||
Matrix actualH(M, M * N);
|
Matrix actualH(M, M * N);
|
||||||
VecD vecd(N, points(0), 0, T);
|
VecD vecd(M, N, points(0), 0, T);
|
||||||
vecd(X, actualH);
|
vecd(X, actualH);
|
||||||
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix<M>, M * N>(
|
Matrix expectedH = numericalDerivative11<Vector1, ParameterMatrix, M * N>(
|
||||||
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
|
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
|
||||||
EXPECT(assert_equal(expectedH, actualH, 1e-6));
|
EXPECT(assert_equal(expectedH, actualH, 1e-6));
|
||||||
}
|
}
|
||||||
|
@ -462,14 +462,14 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
|
||||||
TEST(Chebyshev2, ComponentDerivativeFunctor) {
|
TEST(Chebyshev2, ComponentDerivativeFunctor) {
|
||||||
const size_t N = 6, M = 2;
|
const size_t N = 6, M = 2;
|
||||||
const double x = 0.2;
|
const double x = 0.2;
|
||||||
using CompFunc = Chebyshev2::ComponentDerivativeFunctor<M>;
|
using CompFunc = Chebyshev2::ComponentDerivativeFunctor;
|
||||||
size_t row = 1;
|
size_t row = 1;
|
||||||
CompFunc fx(N, row, x, 0, 3);
|
CompFunc fx(M, N, row, x, 0, 3);
|
||||||
ParameterMatrix<M> X(N);
|
ParameterMatrix X(M, N);
|
||||||
Matrix actualH(1, M * N);
|
Matrix actualH(1, M * N);
|
||||||
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
|
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
|
||||||
|
|
||||||
Matrix expectedH = numericalDerivative11<double, ParameterMatrix<M>, M * N>(
|
Matrix expectedH = numericalDerivative11<double, ParameterMatrix, M * N>(
|
||||||
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
|
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
|
||||||
EXPECT(assert_equal(expectedH, actualH, 1e-7));
|
EXPECT(assert_equal(expectedH, actualH, 1e-7));
|
||||||
}
|
}
|
||||||
|
|
|
@ -180,17 +180,17 @@ TEST(Basis, Derivative7) {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(Basis, VecDerivativeFunctor) {
|
TEST(Basis, VecDerivativeFunctor) {
|
||||||
using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>;
|
using DotShape = typename FourierBasis::VectorDerivativeFunctor;
|
||||||
const size_t N = 3;
|
const size_t N = 3;
|
||||||
|
|
||||||
// MATLAB example, Dec 27 2019, commit 014eded5
|
// MATLAB example, Dec 27 2019, commit 014eded5
|
||||||
double h = 2 * M_PI / 16;
|
double h = 2 * M_PI / 16;
|
||||||
Vector2 dotShape(0.5556, -0.8315); // at h/2
|
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)
|
Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071)
|
||||||
.finished()
|
.finished()
|
||||||
.transpose();
|
.transpose();
|
||||||
ParameterMatrix<2> theta(theta_mat);
|
ParameterMatrix theta(theta_mat);
|
||||||
EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
|
EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -32,19 +32,19 @@ const size_t M = 2, N = 5;
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Constructor) {
|
TEST(ParameterMatrix, Constructor) {
|
||||||
ParameterMatrix<2> actual1(3);
|
ParameterMatrix actual1(2, 3);
|
||||||
ParameterMatrix<2> expected1(Matrix::Zero(2, 3));
|
ParameterMatrix expected1(Matrix::Zero(2, 3));
|
||||||
EXPECT(assert_equal(expected1, actual1));
|
EXPECT(assert_equal(expected1, actual1));
|
||||||
|
|
||||||
ParameterMatrix<2> actual2(Matrix::Ones(2, 3));
|
ParameterMatrix actual2(Matrix::Ones(2, 3));
|
||||||
ParameterMatrix<2> expected2(Matrix::Ones(2, 3));
|
ParameterMatrix expected2(Matrix::Ones(2, 3));
|
||||||
EXPECT(assert_equal(expected2, actual2));
|
EXPECT(assert_equal(expected2, actual2));
|
||||||
EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
|
EXPECT(assert_equal(expected2.matrix(), actual2.matrix()));
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Dimensions) {
|
TEST(ParameterMatrix, Dimensions) {
|
||||||
ParameterMatrix<M> params(N);
|
ParameterMatrix params(M, N);
|
||||||
EXPECT_LONGS_EQUAL(params.rows(), M);
|
EXPECT_LONGS_EQUAL(params.rows(), M);
|
||||||
EXPECT_LONGS_EQUAL(params.cols(), N);
|
EXPECT_LONGS_EQUAL(params.cols(), N);
|
||||||
EXPECT_LONGS_EQUAL(params.dim(), M * N);
|
EXPECT_LONGS_EQUAL(params.dim(), M * N);
|
||||||
|
@ -52,7 +52,7 @@ TEST(ParameterMatrix, Dimensions) {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Getters) {
|
TEST(ParameterMatrix, Getters) {
|
||||||
ParameterMatrix<M> params(N);
|
ParameterMatrix params(M, N);
|
||||||
|
|
||||||
Matrix expectedMatrix = Matrix::Zero(2, 5);
|
Matrix expectedMatrix = Matrix::Zero(2, 5);
|
||||||
EXPECT(assert_equal(expectedMatrix, params.matrix()));
|
EXPECT(assert_equal(expectedMatrix, params.matrix()));
|
||||||
|
@ -60,13 +60,13 @@ TEST(ParameterMatrix, Getters) {
|
||||||
Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
|
Matrix expectedMatrixTranspose = Matrix::Zero(5, 2);
|
||||||
EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
|
EXPECT(assert_equal(expectedMatrixTranspose, params.transpose()));
|
||||||
|
|
||||||
ParameterMatrix<M> p2(Matrix::Ones(M, N));
|
ParameterMatrix p2(Matrix::Ones(M, N));
|
||||||
Vector expectedRowVector = Vector::Ones(N);
|
Vector expectedRowVector = Vector::Ones(N);
|
||||||
for (size_t r = 0; r < M; ++r) {
|
for (size_t r = 0; r < M; ++r) {
|
||||||
EXPECT(assert_equal(p2.row(r), expectedRowVector));
|
EXPECT(assert_equal(p2.row(r), expectedRowVector));
|
||||||
}
|
}
|
||||||
|
|
||||||
ParameterMatrix<M> p3(2 * Matrix::Ones(M, N));
|
ParameterMatrix p3(2 * Matrix::Ones(M, N));
|
||||||
Vector expectedColVector = 2 * Vector::Ones(M);
|
Vector expectedColVector = 2 * Vector::Ones(M);
|
||||||
for (size_t c = 0; c < M; ++c) {
|
for (size_t c = 0; c < M; ++c) {
|
||||||
EXPECT(assert_equal(p3.col(c), expectedColVector));
|
EXPECT(assert_equal(p3.col(c), expectedColVector));
|
||||||
|
@ -75,7 +75,7 @@ TEST(ParameterMatrix, Getters) {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Setters) {
|
TEST(ParameterMatrix, Setters) {
|
||||||
ParameterMatrix<M> params(Matrix::Zero(M, N));
|
ParameterMatrix params(Matrix::Zero(M, N));
|
||||||
Matrix expected = Matrix::Zero(M, N);
|
Matrix expected = Matrix::Zero(M, N);
|
||||||
|
|
||||||
// row
|
// row
|
||||||
|
@ -97,31 +97,31 @@ TEST(ParameterMatrix, Setters) {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Addition) {
|
TEST(ParameterMatrix, Addition) {
|
||||||
ParameterMatrix<M> params(Matrix::Ones(M, N));
|
ParameterMatrix params(Matrix::Ones(M, N));
|
||||||
ParameterMatrix<M> expected(2 * Matrix::Ones(M, N));
|
ParameterMatrix expected(2 * Matrix::Ones(M, N));
|
||||||
|
|
||||||
// Add vector
|
// Add vector
|
||||||
EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
|
EXPECT(assert_equal(expected, params + Vector::Ones(M * N)));
|
||||||
// Add another ParameterMatrix
|
// Add another ParameterMatrix
|
||||||
ParameterMatrix<M> actual = params + ParameterMatrix<M>(Matrix::Ones(M, N));
|
ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N));
|
||||||
EXPECT(assert_equal(expected, actual));
|
EXPECT(assert_equal(expected, actual));
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Subtraction) {
|
TEST(ParameterMatrix, Subtraction) {
|
||||||
ParameterMatrix<M> params(2 * Matrix::Ones(M, N));
|
ParameterMatrix params(2 * Matrix::Ones(M, N));
|
||||||
ParameterMatrix<M> expected(Matrix::Ones(M, N));
|
ParameterMatrix expected(Matrix::Ones(M, N));
|
||||||
|
|
||||||
// Subtract vector
|
// Subtract vector
|
||||||
EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
|
EXPECT(assert_equal(expected, params - Vector::Ones(M * N)));
|
||||||
// Subtract another ParameterMatrix
|
// Subtract another ParameterMatrix
|
||||||
ParameterMatrix<M> actual = params - ParameterMatrix<M>(Matrix::Ones(M, N));
|
ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N));
|
||||||
EXPECT(assert_equal(expected, actual));
|
EXPECT(assert_equal(expected, actual));
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, Multiplication) {
|
TEST(ParameterMatrix, Multiplication) {
|
||||||
ParameterMatrix<M> params(Matrix::Ones(M, N));
|
ParameterMatrix params(Matrix::Ones(M, N));
|
||||||
Matrix multiplier = 2 * Matrix::Ones(N, 2);
|
Matrix multiplier = 2 * Matrix::Ones(N, 2);
|
||||||
Matrix expected = 2 * N * Matrix::Ones(M, 2);
|
Matrix expected = 2 * N * Matrix::Ones(M, 2);
|
||||||
EXPECT(assert_equal(expected, params * multiplier));
|
EXPECT(assert_equal(expected, params * multiplier));
|
||||||
|
@ -129,12 +129,12 @@ TEST(ParameterMatrix, Multiplication) {
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
TEST(ParameterMatrix, VectorSpace) {
|
TEST(ParameterMatrix, VectorSpace) {
|
||||||
ParameterMatrix<M> params(Matrix::Ones(M, N));
|
ParameterMatrix params(Matrix::Ones(M, N));
|
||||||
// vector
|
// vector
|
||||||
EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
|
EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
|
||||||
// identity
|
// identity
|
||||||
EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
|
EXPECT(assert_equal(ParameterMatrix::Identity(M),
|
||||||
ParameterMatrix<M>(Matrix::Zero(M, 0))));
|
ParameterMatrix(Matrix::Zero(M, 0))));
|
||||||
}
|
}
|
||||||
|
|
||||||
//******************************************************************************
|
//******************************************************************************
|
||||||
|
|
|
@ -96,21 +96,7 @@ class Values {
|
||||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
||||||
void insert(size_t j, const gtsam::NavState& nav_state);
|
void insert(size_t j, const gtsam::NavState& nav_state);
|
||||||
void insert(size_t j, double c);
|
void insert(size_t j, double c);
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<1>& X);
|
void insert(size_t j, const gtsam::ParameterMatrix& X);
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<2>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<3>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<4>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<5>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<6>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<7>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<8>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<9>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<10>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<11>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<12>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<13>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
|
|
||||||
void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
|
|
||||||
|
|
||||||
template <T = {gtsam::Point2, gtsam::Point3}>
|
template <T = {gtsam::Point2, gtsam::Point3}>
|
||||||
void insert(size_t j, const T& val);
|
void insert(size_t j, const T& val);
|
||||||
|
@ -144,21 +130,7 @@ class Values {
|
||||||
void update(size_t j, Vector vector);
|
void update(size_t j, Vector vector);
|
||||||
void update(size_t j, Matrix matrix);
|
void update(size_t j, Matrix matrix);
|
||||||
void update(size_t j, double c);
|
void update(size_t j, double c);
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<1>& X);
|
void update(size_t j, const gtsam::ParameterMatrix& X);
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<2>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<3>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<4>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<5>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<6>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<7>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<8>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<9>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<10>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<11>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<12>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<13>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<14>& X);
|
|
||||||
void update(size_t j, const gtsam::ParameterMatrix<15>& X);
|
|
||||||
|
|
||||||
void insert_or_assign(size_t j, const gtsam::Point2& point2);
|
void insert_or_assign(size_t j, const gtsam::Point2& point2);
|
||||||
void insert_or_assign(size_t j, const gtsam::Point3& point3);
|
void insert_or_assign(size_t j, const gtsam::Point3& point3);
|
||||||
|
@ -199,21 +171,7 @@ class Values {
|
||||||
void insert_or_assign(size_t j, Vector vector);
|
void insert_or_assign(size_t j, Vector vector);
|
||||||
void insert_or_assign(size_t j, Matrix matrix);
|
void insert_or_assign(size_t j, Matrix matrix);
|
||||||
void insert_or_assign(size_t j, double c);
|
void insert_or_assign(size_t j, double c);
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X);
|
void insert_or_assign(size_t j, const gtsam::ParameterMatrix& X);
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X);
|
|
||||||
void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X);
|
|
||||||
|
|
||||||
template <T = {gtsam::Point2,
|
template <T = {gtsam::Point2,
|
||||||
gtsam::Point3,
|
gtsam::Point3,
|
||||||
|
@ -244,21 +202,7 @@ class Values {
|
||||||
Vector,
|
Vector,
|
||||||
Matrix,
|
Matrix,
|
||||||
double,
|
double,
|
||||||
gtsam::ParameterMatrix<1>,
|
gtsam::ParameterMatrix}>
|
||||||
gtsam::ParameterMatrix<2>,
|
|
||||||
gtsam::ParameterMatrix<3>,
|
|
||||||
gtsam::ParameterMatrix<4>,
|
|
||||||
gtsam::ParameterMatrix<5>,
|
|
||||||
gtsam::ParameterMatrix<6>,
|
|
||||||
gtsam::ParameterMatrix<7>,
|
|
||||||
gtsam::ParameterMatrix<8>,
|
|
||||||
gtsam::ParameterMatrix<9>,
|
|
||||||
gtsam::ParameterMatrix<10>,
|
|
||||||
gtsam::ParameterMatrix<11>,
|
|
||||||
gtsam::ParameterMatrix<12>,
|
|
||||||
gtsam::ParameterMatrix<13>,
|
|
||||||
gtsam::ParameterMatrix<14>,
|
|
||||||
gtsam::ParameterMatrix<15>}>
|
|
||||||
T at(size_t j);
|
T at(size_t j);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue