RegularImplicitSchurFactor.h
Go to the documentation of this file.
1 
8 #pragma once
9 
13 
14 #include <iosfwd>
15 #include <map>
16 #include <string>
17 #include <vector>
18 
19 namespace gtsam {
20 
38 template<class CAMERA>
40 
41 public:
43  typedef std::shared_ptr<This> shared_ptr;
44 
45 protected:
46 
47  // This factor is closely related to a CameraSet
49 
50  typedef typename CAMERA::Measurement Z;
51  static const int D = traits<CAMERA>::dimension;
52  static const int ZDim = traits<Z>::dimension;
53 
56  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
57 
58  FBlocks FBlocks_;
60  const Matrix E_;
61  const Vector b_;
62 
63 public:
64 
67  }
68 
70 
80  RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
81  const Matrix& E, const Matrix& P, const Vector& b)
82  : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
83 
86  }
87 
88  const FBlocks& Fs() const {
89  return FBlocks_;
90  }
91 
92  const Matrix& E() const {
93  return E_;
94  }
95 
96  const Vector& b() const {
97  return b_;
98  }
99 
100  const Matrix& getPointCovariance() const {
101  return PointCovariance_;
102  }
103 
105  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
106  DefaultKeyFormatter) const override {
107  std::cout << " RegularImplicitSchurFactor " << std::endl;
108  Factor::print(s);
109  for (size_t pos = 0; pos < size(); ++pos) {
110  std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
111  }
112  std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
113  std::cout << "E:\n" << E_ << std::endl;
114  std::cout << "b:\n" << b_.transpose() << std::endl;
115  }
116 
118  bool equals(const GaussianFactor& lf, double tol) const override {
119  const This* f = dynamic_cast<const This*>(&lf);
120  if (!f)
121  return false;
122  for (size_t k = 0; k < FBlocks_.size(); ++k) {
123  if (keys_[k] != f->keys_[k])
124  return false;
125  if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
126  return false;
127  }
128  return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
129  && equal_with_abs_tol(E_, f->E_, tol)
130  && equal_with_abs_tol(b_, f->b_, tol);
131  }
132 
134  DenseIndex getDim(const_iterator variable) const override {
135  return D;
136  }
137 
139  SymmetricBlockMatrix* info) const override {
140  throw std::runtime_error(
141  "RegularImplicitSchurFactor::updateHessian non implemented");
142  }
143  Matrix augmentedJacobian() const override {
144  throw std::runtime_error(
145  "RegularImplicitSchurFactor::augmentedJacobian non implemented");
146  return Matrix();
147  }
148  std::pair<Matrix, Vector> jacobian() const override {
149  throw std::runtime_error(
150  "RegularImplicitSchurFactor::jacobian non implemented");
151  return {Matrix(), Vector()};
152  }
153 
155  Matrix augmentedInformation() const override {
156  // Do the Schur complement
157  SymmetricBlockMatrix augmentedHessian =
158  Set::SchurComplement(FBlocks_, E_, b_);
159  return augmentedHessian.selfadjointView();
160  }
161 
163  Matrix information() const override {
164  Matrix augmented = augmentedInformation();
165  int m = this->keys_.size();
166  size_t M = D * m;
167  return augmented.block(0, 0, M, M);
168  }
169 
172 
174  void hessianDiagonalAdd(VectorValues &d) const override {
175  // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
176  for (size_t k = 0; k < size(); ++k) { // for each camera
177  Key j = keys_[k];
178 
179  // Calculate Fj'*Ej for the current camera (observing a single point)
180  // D x 3 = (D x ZDim) * (ZDim x 3)
181  const MatrixZD& Fj = FBlocks_[k];
182  Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
183  * E_.block<ZDim, 3>(ZDim * k, 0);
184 
186  for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
187  // Vector column_k_Fj = Fj.col(k);
188  dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
189  // Vector column_k_FtE = FtE.row(k);
190  // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
191  dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
192  }
193 
194  auto result = d.emplace(j, dj);
195  if(!result.second) {
196  result.first->second += dj;
197  }
198  }
199  }
200 
205  void hessianDiagonal(double* d) const override {
206  // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
207  // Use eigen magic to access raw memory
208  typedef Eigen::Matrix<double, D, 1> DVector;
209  typedef Eigen::Map<DVector> DMap;
210 
211  for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor
212  Key j = keys_[pos];
213 
214  // Calculate Fj'*Ej for the current camera (observing a single point)
215  // D x 3 = (D x ZDim) * (ZDim x 3)
216  const MatrixZD& Fj = FBlocks_[pos];
217  Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
218  * E_.block<ZDim, 3>(ZDim * pos, 0);
219 
220  DVector dj;
221  for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
222  dj(k) = Fj.col(k).squaredNorm();
223  // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
224  dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
225  }
226  DMap(d + D * j) += dj;
227  }
228  }
229 
231  std::map<Key, Matrix> hessianBlockDiagonal() const override {
232  std::map<Key, Matrix> blocks;
233  // F'*(I - E*P*E')*F
234  for (size_t pos = 0; pos < size(); ++pos) {
235  Key j = keys_[pos];
236  // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
237  const MatrixZD& Fj = FBlocks_[pos];
238  // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
239  // * E_.block<ZDim, 3>(ZDim * pos, 0);
240  // blocks[j] = Fj.transpose() * Fj
241  // - FtE * PointCovariance_ * FtE.transpose();
242 
243  const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
244  blocks[j] = Fj.transpose()
245  * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
246 
247  // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
248  // static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
249  // Matrix2 Q = //
250  // I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
251  // blocks[j] = Fj.transpose() * Q * Fj;
252  }
253  return blocks;
254  }
255 
257  return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
259  throw std::runtime_error(
260  "RegularImplicitSchurFactor::clone non implemented");
261  }
262 
264  return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
266  throw std::runtime_error(
267  "RegularImplicitSchurFactor::negate non implemented");
268  }
269 
270  // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
271  static
272  void multiplyHessianAdd(const Matrix& F, const Matrix& E,
273  const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
274  Vector e1 = F * x;
275  Vector d1 = E.transpose() * e1;
276  Vector d2 = PointCovariance * d1;
277  Vector e2 = E * d2;
278  Vector e3 = alpha * (e1 - e2);
279  y += F.transpose() * e3;
280  }
281 
282  typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
283 
287  void projectError2(const Error2s& e1, Error2s& e2) const {
288 
289  // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
290  Vector3 d1;
291  d1.setZero();
292  for (size_t k = 0; k < size(); k++)
293  d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
294  * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
295 
296  // d2 = E.transpose() * e1 = (3*2m)*2m
297  Vector3 d2 = PointCovariance_ * d1;
298 
299  // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
300  for (size_t k = 0; k < size(); k++)
301  e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
302  - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
303  }
304 
305  /*
306  * This definition matches the linearized error in the Hessian Factor:
307  * LinError(x) = x'*H*x - 2*x'*eta + f
308  * with:
309  * H = F' * (I-E'*P*E) * F = F' * Q * F
310  * eta = F' * (I-E'*P*E) * b = F' * Q * b
311  * f = nonlinear error
312  * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
313  */
314  double error(const VectorValues& x) const override {
315 
316  // resize does not do malloc if correct size
317  e1.resize(size());
318  e2.resize(size());
319 
320  // e1 = F * x - b = (2m*dm)*dm
321  for (size_t k = 0; k < size(); ++k)
322  e1[k] = FBlocks_[k] * x.at(keys_[k]);
323  projectError2(e1, e2);
324 
325  double result = 0;
326  for (size_t k = 0; k < size(); ++k)
327  result += dot(e1[k], e2[k]);
328 
329  double f = b_.squaredNorm();
330  return 0.5 * (result + f);
331  }
332 
333  // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
334  // This is wrong and does not match the definition in Hessian,
335  // but it matches the definition of the Jacobian factor (JF)
336  double errorJF(const VectorValues& x) const {
337 
338  // resize does not do malloc if correct size
339  e1.resize(size());
340  e2.resize(size());
341 
342  // e1 = F * x - b = (2m*dm)*dm
343  for (size_t k = 0; k < size(); ++k)
344  e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
345  projectError(e1, e2);
346 
347  double result = 0;
348  for (size_t k = 0; k < size(); ++k)
349  result += dot(e2[k], e2[k]);
350 
351  // std::cout << "implicitFactor::error result " << result << std::endl;
352  return 0.5 * result;
353  }
357  void projectError(const Error2s& e1, Error2s& e2) const {
358 
359  // d1 = E.transpose() * e1 = (3*2m)*2m
360  Vector3 d1;
361  d1.setZero();
362  for (size_t k = 0; k < size(); k++)
363  d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
364 
365  // d2 = E.transpose() * e1 = (3*2m)*2m
366  Vector3 d2 = PointCovariance_ * d1;
367 
368  // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
369  for (size_t k = 0; k < size(); k++)
370  e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
371  }
372 
374  mutable Error2s e1, e2;
375 
380  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
381 
382  // Use eigen magic to access raw memory
383  typedef Eigen::Matrix<double, D, 1> DVector;
384  typedef Eigen::Map<DVector> DMap;
385  typedef Eigen::Map<const DVector> ConstDMap;
386 
387  // resize does not do malloc if correct size
388  e1.resize(size());
389  e2.resize(size());
390 
391  // e1 = F * x = (2m*dm)*dm
392  for (size_t k = 0; k < size(); ++k) {
393  Key key = keys_[k];
394  e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
395  }
396 
397  projectError(e1, e2);
398 
399  // y += F.transpose()*e2 = (2d*2m)*2m
400  for (size_t k = 0; k < size(); ++k) {
401  Key key = keys_[k];
402  DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
403  }
404  }
405 
406  void multiplyHessianAdd(double alpha, const double* x, double* y,
407  std::vector<size_t> keys) const {
408  }
409 
413  void multiplyHessianAdd(double alpha, const VectorValues& x,
414  VectorValues& y) const override {
415 
416  // resize does not do malloc if correct size
417  e1.resize(size());
418  e2.resize(size());
419 
420  // e1 = F * x = (2m*dm)*dm
421  for (size_t k = 0; k < size(); ++k)
422  e1[k] = FBlocks_[k] * x.at(keys_[k]);
423 
424  projectError(e1, e2);
425 
426  // y += F.transpose()*e2 = (2d*2m)*2m
427  for (size_t k = 0; k < size(); ++k) {
428  Key key = keys_[k];
429  static const Vector empty;
430  std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
431  Vector& yi = it.first->second;
432  // Create the value as a zero vector if it does not exist.
433  if (it.second)
434  yi = Vector::Zero(FBlocks_[k].cols());
435  yi += FBlocks_[k].transpose() * alpha * e2[k];
436  }
437  }
438 
443  VectorValues& y) const {
444 
445  for (size_t k = 0; k < size(); ++k) {
446  static const Vector empty;
447  Key key = keys_[k];
448  std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
449  Vector& yi = it.first->second;
450  yi = x.at(key);
451  }
452  }
453 
457  VectorValues gradientAtZero() const override {
458  // calculate Q*b
459  e1.resize(size());
460  e2.resize(size());
461  for (size_t k = 0; k < size(); k++)
462  e1[k] = b_.segment<ZDim>(ZDim * k);
463  projectError(e1, e2);
464 
465  // g = F.transpose()*e2
466  VectorValues g;
467  for (size_t k = 0; k < size(); ++k) {
468  Key key = keys_[k];
469  g.insert(key, -FBlocks_[k].transpose() * e2[k]);
470  }
471 
472  // return it
473  return g;
474  }
475 
479  void gradientAtZero(double* d) const override {
480 
481  // Use eigen magic to access raw memory
482  typedef Eigen::Matrix<double, D, 1> DVector;
483  typedef Eigen::Map<DVector> DMap;
484 
485  // calculate Q*b
486  e1.resize(size());
487  e2.resize(size());
488  for (size_t k = 0; k < size(); k++)
489  e1[k] = b_.segment<ZDim>(ZDim * k);
490  projectError(e1, e2);
491 
492  for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
493  Key j = keys_[k];
494  DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
495  }
496  }
497 
499  Vector gradient(Key key, const VectorValues& x) const override {
500  throw std::runtime_error(
501  "gradient for RegularImplicitSchurFactor is not implemented yet");
502  }
503 
504 };
505 // end class RegularImplicitSchurFactor
506 
507 template<class CAMERA>
509 
510 template<class CAMERA>
512 
513 // traits
514 template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
515  RegularImplicitSchurFactor<CAMERA> > {
516 };
517 
518 }
519 
const gtsam::Symbol key('X', 0)
Matrix3f m
static const int D
Camera dimension.
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:130
double errorJF(const VectorValues &x) const
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e. y += F&#39;alpha(I - E*P*E&#39;)*F*x RAW memory access! Assumes keys st...
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
void multiplyHessianAdd(double alpha, const double *x, double *y, std::vector< size_t > keys) const
Scalar * y
Key F(std::uint64_t j)
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Eigen::Vector3d Vector3
Definition: Vector.h:43
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
static void multiplyHessianAdd(const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y)
Base class to create smart factors on poses or cameras.
GaussianFactor::shared_ptr negate() const override
size_t size() const
Definition: Factor.h:159
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
GaussianFactor::shared_ptr clone() const override
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:87
iterator insert(const std::pair< Key, Vector > &key_value)
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
FBlocks FBlocks_
All ZDim*D F blocks (one for each camera)
Vector & at(Key j)
Definition: VectorValues.h:139
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND >>> &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Definition: CameraSet.h:171
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
void g(const string &key, int i)
Definition: testBTree.cpp:41
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
Definition: VectorValues.h:209
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e. y += F&#39;alpha(I - E*P*E&#39;)*F*x.
double error(const VectorValues &x) const override
Error2s e1
Scratch space for multiplyHessianAdd.
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
RegularImplicitSchurFactor This
Typedef to this class.
RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b)
Construct from blocks of F, E, inv(E&#39;*E), and RHS vector b.
else if n * info
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E&#39;)*(e-ZDim*b)
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Factor Graph Values.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
std::vector< Vector2, Eigen::aligned_allocator< Vector2 > > Error2s
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E&#39;)*e.
RealScalar alpha
const Matrix PointCovariance_
the 3*3 matrix P = inv(E&#39;E) (2*2 if degenerate)
RealScalar s
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
std::shared_ptr< This > shared_ptr
shared_ptr to this class
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
traits
Definition: chartTesting.h:28
void gradientAtZero(double *d) const override
~RegularImplicitSchurFactor() override
Destructor.
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
VectorValues gradientAtZero() const override
Matrix information() const override
Compute full information matrix
static const int ZDim
Measurement dimension.
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
bool equals(const GaussianFactor &lf, double tol) const override
equals
const G double tol
Definition: Group.h:86
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
The matrix class, also used for vectors and row-vectors.
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
const Vector b_
2m-dimensional RHS vector
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Matrix augmentedInformation() const override
Compute full augmented information matrix
std::ptrdiff_t j
std::pair< Matrix, Vector > jacobian() const override


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:32