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>
23 
24 #include <iostream>
25 
67 namespace gtsam {
68 
69 using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
70 
83 Matrix kroneckerProductIdentity(size_t M, const Weights& w);
84 
89 template <typename DERIVED>
90 class Basis {
91  public:
97  static Matrix WeightMatrix(size_t N, const Vector& X) {
98  Matrix W(X.size(), N);
99  for (int i = 0; i < X.size(); i++)
100  W.row(i) = DERIVED::CalculateWeights(N, X(i));
101  return W;
102  }
103 
113  static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
114  Matrix W(X.size(), N);
115  for (int i = 0; i < X.size(); i++)
116  W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
117  return W;
118  }
119 
128  protected:
130 
131  public:
134 
136  EvaluationFunctor(size_t N, double x)
137  : weights_(DERIVED::CalculateWeights(N, x)) {}
138 
140  EvaluationFunctor(size_t N, double x, double a, double b)
141  : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
142 
144  double apply(const typename DERIVED::Parameters& p,
145  OptionalJacobian<-1, -1> H = {}) const {
146  if (H) *H = weights_;
147  return (weights_ * p)(0);
148  }
149 
151  double operator()(const typename DERIVED::Parameters& p,
152  OptionalJacobian<-1, -1> H = {}) const {
153  return apply(p, H); // might call apply in derived
154  }
155 
156  void print(const std::string& s = "") const {
157  std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
158  }
159  };
160 
168  protected:
169  using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
171 
172  size_t M_;
173 
183  H_ = kroneckerProductIdentity(M_, this->weights_);
184  }
185 
186  public:
188 
191 
193  VectorEvaluationFunctor(size_t M, size_t N, double x)
194  : EvaluationFunctor(N, x), M_(M) {
195  calculateJacobian();
196  }
197 
199  VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
200  : EvaluationFunctor(N, x, a, b), M_(M) {
201  calculateJacobian();
202  }
203 
206  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
207  if (H) *H = H_;
208  return P.matrix() * this->weights_.transpose();
209  }
210 
213  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
214  return apply(P, H);
215  }
216  };
217 
226  protected:
227  using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
229 
230  size_t M_;
231  size_t rowIndex_;
232 
233  /*
234  * Calculate the `1*(M*N)` Jacobian of this functor with respect to
235  * the M*N parameter matrix `P`.
236  * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
237  * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
238  * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
239  * i.e., one row of the Kronecker product of weights_ with the
240  * MxM identity matrix. See also VectorEvaluationFunctor.
241  */
242  void calculateJacobian(size_t N) {
243  H_.setZero(1, M_ * N);
244  for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
245  H_(0, rowIndex_ + j * M_) = EvaluationFunctor::weights_(j);
246  }
247 
248  public:
251 
253  VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
254  : EvaluationFunctor(N, x), M_(M), rowIndex_(i) {
255  calculateJacobian(N);
256  }
257 
259  VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a,
260  double b)
261  : EvaluationFunctor(N, x, a, b), M_(M), rowIndex_(i) {
262  calculateJacobian(N);
263  }
264 
266  double apply(const Matrix& P,
268  if (H) *H = H_;
269  return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
270  }
271 
273  double operator()(const Matrix& P,
275  return apply(P, H);
276  }
277  };
278 
292  template <class T>
296 
297  public:
300 
302  ManifoldEvaluationFunctor(size_t N, double x) : Base(M, N, x) {}
303 
305  ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
306  : Base(M, N, x, a, b) {}
307 
310  // Interpolate the M-dimensional vector to yield a vector in tangent space
312 
313  // Now call retract with this M-vector, possibly with derivatives
314  Eigen::Matrix<double, M, M> D_result_xi;
315  T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
316 
317  // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
318  // derivative of interpolation and D_result_xi is MxM derivative of
319  // retract.
320  if (H) *H = D_result_xi * (*H);
321 
322  // and return a T
323  return result;
324  }
325 
327  T operator()(const Matrix& P,
328  OptionalJacobian</*MxN*/ -1, -1> H = {}) const {
329  return apply(P, H); // might call apply in derived
330  }
331  };
332 
335  protected:
337 
338  public:
341 
342  DerivativeFunctorBase(size_t N, double x)
343  : weights_(DERIVED::DerivativeWeights(N, x)) {}
344 
345  DerivativeFunctorBase(size_t N, double x, double a, double b)
346  : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
347 
348  void print(const std::string& s = "") const {
349  std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
350  }
351  };
352 
361  public:
364 
365  DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
366 
367  DerivativeFunctor(size_t N, double x, double a, double b)
368  : DerivativeFunctorBase(N, x, a, b) {}
369 
370  double apply(const typename DERIVED::Parameters& p,
371  OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
372  if (H) *H = this->weights_;
373  return (this->weights_ * p)(0);
374  }
376  double operator()(const typename DERIVED::Parameters& p,
377  OptionalJacobian</*1xN*/ -1, -1> H = {}) const {
378  return apply(p, H); // might call apply in derived
379  }
380  };
381 
391  protected:
392  using Jacobian = Eigen::Matrix<double, /*MxMN*/ -1, -1>;
394 
395  size_t M_;
396 
406  H_ = kroneckerProductIdentity(M_, this->weights_);
407  }
408 
409  public:
411 
414 
416  VectorDerivativeFunctor(size_t M, size_t N, double x)
417  : DerivativeFunctorBase(N, x), M_(M) {
418  calculateJacobian();
419  }
420 
422  VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
423  : DerivativeFunctorBase(N, x, a, b), M_(M) {
424  calculateJacobian();
425  }
426 
427  Vector apply(const Matrix& P,
429  if (H) *H = H_;
430  return P.matrix() * this->weights_.transpose();
431  }
435  return apply(P, H);
436  }
437  };
438 
447  protected:
448  using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
450 
451  size_t M_;
452  size_t rowIndex_;
453 
454  /*
455  * Calculate the `1*(M*N)` Jacobian of this functor with respect to
456  * the M*N parameter matrix `P`.
457  * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
458  * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
459  * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
460  * i.e., one row of the Kronecker product of weights_ with the
461  * MxM identity matrix. See also VectorDerivativeFunctor.
462  */
463  void calculateJacobian(size_t N) {
464  H_.setZero(1, M_ * N);
465  for (int j = 0; j < this->weights_.size(); j++)
466  H_(0, rowIndex_ + j * M_) = this->weights_(j);
467  }
468 
469  public:
472 
474  ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
475  : DerivativeFunctorBase(N, x), M_(M), rowIndex_(i) {
476  calculateJacobian(N);
477  }
478 
480  ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a,
481  double b)
482  : DerivativeFunctorBase(N, x, a, b), M_(M), rowIndex_(i) {
483  calculateJacobian(N);
484  }
486  double apply(const Matrix& P,
488  if (H) *H = H_;
489  return P.row(rowIndex_) * this->weights_.transpose();
490  }
492  double operator()(const Matrix& P,
494  return apply(P, H);
495  }
496  };
497 };
498 
499 } // namespace gtsam
void print(const std::string &s="") const
Definition: Basis.h:156
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Matrix kroneckerProductIdentity(size_t M, const Weights &w)
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix ...
Definition: Basis.cpp:23
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition: Basis.h:136
DerivativeFunctor()
For serialization.
Definition: Basis.h:363
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
Key W(std::uint64_t j)
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition: Basis.h:480
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition: Basis.h:190
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
VectorComponentFunctor(size_t M, size_t N, size_t i, double x)
Construct with row index.
Definition: Basis.h:253
#define N
Definition: gksort.c:12
T apply(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
Manifold evaluation.
Definition: Basis.h:309
DerivativeFunctor(size_t N, double x)
Definition: Basis.h:365
double apply(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
Calculate derivative of component rowIndex_ of F.
Definition: Basis.h:486
DerivativeFunctorBase()
For serialization.
Definition: Basis.h:340
VectorEvaluationFunctor(size_t M, size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition: Basis.h:199
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition: Basis.h:302
VectorComponentFunctor(size_t M, size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition: Basis.h:259
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition: Basis.h:140
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
DerivativeFunctorBase(size_t N, double x, double a, double b)
Definition: Basis.h:345
double operator()(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:492
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition: Basis.h:305
void print(const std::string &s="") const
Definition: Basis.h:348
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:113
RealScalar s
double apply(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
Calculate component of component rowIndex_ of P.
Definition: Basis.h:266
DerivativeFunctorBase(size_t N, double x)
Definition: Basis.h:342
const G & b
Definition: Group.h:86
RowVector3d w
Vector xi
Definition: testPose2.cpp:148
VectorEvaluationFunctor(size_t M, size_t N, double x)
Default Constructor.
Definition: Basis.h:193
traits
Definition: chartTesting.h:28
Vector apply(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
Definition: Basis.h:427
DerivativeFunctor(size_t N, double x, double a, double b)
Definition: Basis.h:367
ManifoldEvaluationFunctor()
For serialization.
Definition: Basis.h:299
EvaluationFunctor()
For serialization.
Definition: Basis.h:133
ComponentDerivativeFunctor(size_t M, size_t N, size_t i, double x)
Construct with row index.
Definition: Basis.h:474
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition: Basis.h:413
static Matrix WeightMatrix(size_t N, const Vector &X)
Definition: Basis.h:97
VectorDerivativeFunctor(size_t M, size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition: Basis.h:422
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:376
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H={}) const
Regular 1D evaluation.
Definition: Basis.h:144
float * p
VectorDerivativeFunctor(size_t M, size_t N, double x)
Default Constructor.
Definition: Basis.h:416
ComponentDerivativeFunctor()
For serialization.
Definition: Basis.h:471
Vector operator()(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:212
VectorComponentFunctor()
For serialization.
Definition: Basis.h:250
Vector apply(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
M-dimensional evaluation.
Definition: Basis.h:205
double apply(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H={}) const
Definition: Basis.h:370
Vector operator()(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:433
Special class for optional Jacobian arguments.
double operator()(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:273
The matrix class, also used for vectors and row-vectors.
#define X
Definition: icosphere.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
std::ptrdiff_t j
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H={}) const
c++ sugar
Definition: Basis.h:151
Base class for functors below that calculate derivative weights.
Definition: Basis.h:334
T operator()(const Matrix &P, OptionalJacobian< -1, -1 > H={}) const
c++ sugar
Definition: Basis.h:327
void calculateJacobian(size_t N)
Definition: Basis.h:242


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:57