70using Weights = Eigen::Matrix<double, 1, -1>;
86 Matrix result(M, w.cols() * M);
89 for (
int i = 0; i < w.cols(); i++) {
90 result.block(0, i * M, M, M).diagonal().array() = w(i);
99template <
typename DERIVED>
108 Matrix W(X.size(), N);
109 for (
int i = 0; i < X.size(); i++)
110 W.row(i) = DERIVED::CalculateWeights(N, X(i));
123 static Matrix
WeightMatrix(
size_t N,
const Vector& X,
double a,
double b) {
124 Matrix W(X.size(), N);
125 for (
int i = 0; i < X.size(); i++)
126 W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
147 : weights_(DERIVED::CalculateWeights(N, x)) {}
151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
154 double apply(
const typename DERIVED::Parameters& p,
156 if (H) *H = weights_;
157 return (weights_ * p)(0);
166 void print(
const std::string& s =
"")
const {
167 std::cout << s << (s !=
"" ?
" " :
"") << weights_ << std::endl;
180 using VectorM = Eigen::Matrix<double, M, 1>;
181 using Jacobian = Eigen::Matrix<double, M, -1>;
193 H_ = kroneckerProductIdentity<M>(this->weights_);
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
217 return P.matrix() * this->weights_.transpose();
237 using Jacobian = Eigen::Matrix<double, 1, -1>;
250 void calculateJacobian(
size_t N) {
251 H_.setZero(1, M * N);
252 for (
int j = 0; j < EvaluationFunctor::weights_.size(); j++)
253 H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
263 calculateJacobian(N);
269 calculateJacobian(N);
276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
314 : Base(N, x, a, b) {}
320 Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
323 Eigen::Matrix<double, M, M> D_result_xi;
324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
329 if (H) *H = D_result_xi * (*H);
352 : weights_(DERIVED::DerivativeWeights(N, x)) {}
355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
357 void print(
const std::string& s =
"")
const {
358 std::cout << s << (s !=
"" ?
" " :
"") << weights_ << std::endl;
379 double apply(
const typename DERIVED::Parameters& p,
380 OptionalJacobian</*1xN*/ -1, -1> H = boost::none)
const {
381 if (H) *H = this->weights_;
382 return (this->weights_ * p)(0);
402 using VectorM = Eigen::Matrix<double, M, 1>;
403 using Jacobian = Eigen::Matrix<double, M, -1>;
415 H_ = kroneckerProductIdentity<M>(this->weights_);
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
438 return P.matrix() * this->weights_.transpose();
458 using Jacobian = Eigen::Matrix<double, 1, -1>;
471 void calculateJacobian(
size_t N) {
472 H_.setZero(1, M * N);
473 for (
int j = 0; j < this->weights_.size(); j++)
474 H_(0, rowIndex_ + j * M) = this->weights_(j);
484 calculateJacobian(N);
490 calculateJacobian(N);
496 return P.row(rowIndex_) * this->weights_.transpose();
Special class for optional Jacobian arguments.
typedef and functions to augment Eigen's MatrixXd
Define ParameterMatrix class which is used to store values at interpolation points.
Matrix kroneckerProductIdentity(const Weights &w)
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix ...
Definition Basis.h:85
Global functions in a separate testing namespace.
Definition chartTesting.h:28
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
CRTP Base class for function bases.
Definition Basis.h:100
static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b)
Calculate weights for all x in vector X, with interval [a,b].
Definition Basis.h:123
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p.
Definition Basis.h:137
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
EvaluationFunctor()
For serialization.
Definition Basis.h:143
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:178
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor compute...
Definition Basis.h:235
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:301
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p.
Definition Basis.h:369
DerivativeFunctor()
For serialization.
Definition Basis.h:372
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:400
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor com...
Definition Basis.h:456
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38