gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
Basis.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
21#include <gtsam/base/Matrix.h>
24
25#include <iostream>
26
68namespace gtsam {
69
70using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
71
84template <size_t M>
85Matrix kroneckerProductIdentity(const Weights& w) {
86 Matrix result(M, w.cols() * M);
87 result.setZero();
88
89 for (int i = 0; i < w.cols(); i++) {
90 result.block(0, i * M, M, M).diagonal().array() = w(i);
91 }
92 return result;
93}
94
99template <typename DERIVED>
100class Basis {
101 public:
107 static Matrix WeightMatrix(size_t N, const Vector& X) {
108 Matrix W(X.size(), N);
109 for (int i = 0; i < X.size(); i++)
110 W.row(i) = DERIVED::CalculateWeights(N, X(i));
111 return W;
112 }
113
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);
127 return W;
128 }
129
138 protected:
139 Weights weights_;
140
141 public:
144
146 EvaluationFunctor(size_t N, double x)
147 : weights_(DERIVED::CalculateWeights(N, x)) {}
148
150 EvaluationFunctor(size_t N, double x, double a, double b)
151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
152
154 double apply(const typename DERIVED::Parameters& p,
155 OptionalJacobian<-1, -1> H = boost::none) const {
156 if (H) *H = weights_;
157 return (weights_ * p)(0);
158 }
159
161 double operator()(const typename DERIVED::Parameters& p,
162 OptionalJacobian<-1, -1> H = boost::none) const {
163 return apply(p, H); // might call apply in derived
164 }
165
166 void print(const std::string& s = "") const {
167 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
168 }
169 };
170
177 template <int M>
179 protected:
180 using VectorM = Eigen::Matrix<double, M, 1>;
181 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
182 Jacobian H_;
183
193 H_ = kroneckerProductIdentity<M>(this->weights_);
194 }
195
196 public:
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
198
201
203 VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
205 }
206
208 VectorEvaluationFunctor(size_t N, double x, double a, double b)
209 : EvaluationFunctor(N, x, a, b) {
211 }
212
214 VectorM apply(const ParameterMatrix<M>& P,
215 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
216 if (H) *H = H_;
217 return P.matrix() * this->weights_.transpose();
218 }
219
222 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
223 return apply(P, H);
224 }
225 };
226
234 template <int M>
236 protected:
237 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
238 size_t rowIndex_;
239 Jacobian H_;
240
241 /*
242 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
243 * the M*N parameter matrix `P`.
244 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
245 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
246 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
247 * i.e., one row of the Kronecker product of weights_ with the
248 * MxM identity matrix. See also VectorEvaluationFunctor.
249 */
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);
254 }
255
256 public:
259
261 VectorComponentFunctor(size_t N, size_t i, double x)
262 : EvaluationFunctor(N, x), rowIndex_(i) {
263 calculateJacobian(N);
264 }
265
267 VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
268 : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
269 calculateJacobian(N);
270 }
271
273 double apply(const ParameterMatrix<M>& P,
274 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
275 if (H) *H = H_;
276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
277 }
278
281 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
282 return apply(P, H);
283 }
284 };
285
299 template <class T>
301 : public VectorEvaluationFunctor<traits<T>::dimension> {
302 enum { M = traits<T>::dimension };
303 using Base = VectorEvaluationFunctor<M>;
304
305 public:
308
310 ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
311
313 ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
314 : Base(N, x, a, b) {}
315
318 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
319 // Interpolate the M-dimensional vector to yield a vector in tangent space
320 Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
321
322 // Now call retract with this M-vector, possibly with derivatives
323 Eigen::Matrix<double, M, M> D_result_xi;
324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
325
326 // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
327 // derivative of interpolation and D_result_xi is MxM derivative of
328 // retract.
329 if (H) *H = D_result_xi * (*H);
330
331 // and return a T
332 return result;
333 }
334
337 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
338 return apply(P, H); // might call apply in derived
339 }
340 };
341
344 protected:
345 Weights weights_;
346
347 public:
350
351 DerivativeFunctorBase(size_t N, double x)
352 : weights_(DERIVED::DerivativeWeights(N, x)) {}
353
354 DerivativeFunctorBase(size_t N, double x, double a, double b)
355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
356
357 void print(const std::string& s = "") const {
358 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
359 }
360 };
361
370 public:
373
374 DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
375
376 DerivativeFunctor(size_t N, double x, double a, double b)
377 : DerivativeFunctorBase(N, x, a, b) {}
378
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);
383 }
385 double operator()(const typename DERIVED::Parameters& p,
386 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
387 return apply(p, H); // might call apply in derived
388 }
389 };
390
399 template <int M>
401 protected:
402 using VectorM = Eigen::Matrix<double, M, 1>;
403 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
404 Jacobian H_;
405
415 H_ = kroneckerProductIdentity<M>(this->weights_);
416 }
417
418 public:
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
420
423
425 VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
427 }
428
430 VectorDerivativeFunctor(size_t N, double x, double a, double b)
431 : DerivativeFunctorBase(N, x, a, b) {
433 }
434
435 VectorM apply(const ParameterMatrix<M>& P,
436 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
437 if (H) *H = H_;
438 return P.matrix() * this->weights_.transpose();
439 }
441 VectorM operator()(
442 const ParameterMatrix<M>& P,
443 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
444 return apply(P, H);
445 }
446 };
447
455 template <int M>
457 protected:
458 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
459 size_t rowIndex_;
460 Jacobian H_;
461
462 /*
463 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
464 * the M*N parameter matrix `P`.
465 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
466 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
467 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
468 * i.e., one row of the Kronecker product of weights_ with the
469 * MxM identity matrix. See also VectorDerivativeFunctor.
470 */
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);
475 }
476
477 public:
480
482 ComponentDerivativeFunctor(size_t N, size_t i, double x)
483 : DerivativeFunctorBase(N, x), rowIndex_(i) {
484 calculateJacobian(N);
485 }
486
488 ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
489 : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
490 calculateJacobian(N);
491 }
493 double apply(const ParameterMatrix<M>& P,
494 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
495 if (H) *H = H_;
496 return P.row(rowIndex_) * this->weights_.transpose();
497 }
500 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
501 return apply(P, H);
502 }
503 };
504
505};
506
507} // namespace gtsam
typedef and functions to augment Eigen's MatrixXd
Special class for optional Jacobian arguments.
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