gtsam  4.1.0
gtsam
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 
24 template<class CAMERA>
26 
27 public:
29  typedef boost::shared_ptr<This> shared_ptr;
30 
31 protected:
32 
33  // This factor is closely related to a CameraSet
34  typedef CameraSet<CAMERA> Set;
35 
36  typedef typename CAMERA::Measurement Z;
37  static const int D = traits<CAMERA>::dimension;
38  static const int ZDim = traits<Z>::dimension;
39 
40  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
41  typedef Eigen::Matrix<double, D, D> MatrixDD;
42 
43  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_;
44  const Matrix PointCovariance_;
45  const Matrix E_;
46  const Vector b_;
47 
48 public:
49 
52  }
53 
56  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
57  const Vector& b) :
58  GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
59  }
60 
63  }
64 
65  std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
66  return FBlocks_;
67  }
68 
69  const Matrix& E() const {
70  return E_;
71  }
72 
73  const Vector& b() const {
74  return b_;
75  }
76 
77  const Matrix& getPointCovariance() const {
78  return PointCovariance_;
79  }
80 
82  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
83  DefaultKeyFormatter) const override {
84  std::cout << " RegularImplicitSchurFactor " << std::endl;
85  Factor::print(s);
86  for (size_t pos = 0; pos < size(); ++pos) {
87  std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
88  }
89  std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
90  std::cout << "E:\n" << E_ << std::endl;
91  std::cout << "b:\n" << b_.transpose() << std::endl;
92  }
93 
95  bool equals(const GaussianFactor& lf, double tol) const override {
96  const This* f = dynamic_cast<const This*>(&lf);
97  if (!f)
98  return false;
99  for (size_t k = 0; k < FBlocks_.size(); ++k) {
100  if (keys_[k] != f->keys_[k])
101  return false;
102  if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
103  return false;
104  }
105  return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
106  && equal_with_abs_tol(E_, f->E_, tol)
107  && equal_with_abs_tol(b_, f->b_, tol);
108  }
109 
111  DenseIndex getDim(const_iterator variable) const override {
112  return D;
113  }
114 
116  SymmetricBlockMatrix* info) const override {
117  throw std::runtime_error(
118  "RegularImplicitSchurFactor::updateHessian non implemented");
119  }
120  Matrix augmentedJacobian() const override {
121  throw std::runtime_error(
122  "RegularImplicitSchurFactor::augmentedJacobian non implemented");
123  return Matrix();
124  }
125  std::pair<Matrix, Vector> jacobian() const override {
126  throw std::runtime_error(
127  "RegularImplicitSchurFactor::jacobian non implemented");
128  return std::make_pair(Matrix(), Vector());
129  }
130 
132  Matrix augmentedInformation() const override {
133  // Do the Schur complement
134  SymmetricBlockMatrix augmentedHessian =
136  return augmentedHessian.selfadjointView();
137  }
138 
140  Matrix information() const override {
141  Matrix augmented = augmentedInformation();
142  int m = this->keys_.size();
143  size_t M = D * m;
144  return augmented.block(0, 0, M, M);
145  }
146 
149 
151  void hessianDiagonalAdd(VectorValues &d) const override {
152  // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
153  for (size_t k = 0; k < size(); ++k) { // for each camera
154  Key j = keys_[k];
155 
156  // Calculate Fj'*Ej for the current camera (observing a single point)
157  // D x 3 = (D x ZDim) * (ZDim x 3)
158  const MatrixZD& Fj = FBlocks_[k];
159  Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
160  * E_.block<ZDim, 3>(ZDim * k, 0);
161 
162  Eigen::Matrix<double, D, 1> dj;
163  for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
164  // Vector column_k_Fj = Fj.col(k);
165  dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
166  // Vector column_k_FtE = FtE.row(k);
167  // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
168  dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
169  }
170 
171  auto result = d.emplace(j, dj);
172  if(!result.second) {
173  result.first->second += dj;
174  }
175  }
176  }
177 
182  void hessianDiagonal(double* d) const override {
183  // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
184  // Use eigen magic to access raw memory
185  typedef Eigen::Matrix<double, D, 1> DVector;
186  typedef Eigen::Map<DVector> DMap;
187 
188  for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor
189  Key j = keys_[pos];
190 
191  // Calculate Fj'*Ej for the current camera (observing a single point)
192  // D x 3 = (D x ZDim) * (ZDim x 3)
193  const MatrixZD& Fj = FBlocks_[pos];
194  Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
195  * E_.block<ZDim, 3>(ZDim * pos, 0);
196 
197  DVector dj;
198  for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
199  dj(k) = Fj.col(k).squaredNorm();
200  // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
201  dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
202  }
203  DMap(d + D * j) += dj;
204  }
205  }
206 
208  std::map<Key, Matrix> hessianBlockDiagonal() const override {
209  std::map<Key, Matrix> blocks;
210  // F'*(I - E*P*E')*F
211  for (size_t pos = 0; pos < size(); ++pos) {
212  Key j = keys_[pos];
213  // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
214  const MatrixZD& Fj = FBlocks_[pos];
215  // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
216  // * E_.block<ZDim, 3>(ZDim * pos, 0);
217  // blocks[j] = Fj.transpose() * Fj
218  // - FtE * PointCovariance_ * FtE.transpose();
219 
220  const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
221  blocks[j] = Fj.transpose()
222  * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
223 
224  // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
225  // static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
226  // Matrix2 Q = //
227  // I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
228  // blocks[j] = Fj.transpose() * Q * Fj;
229  }
230  return blocks;
231  }
232 
234  return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
236  throw std::runtime_error(
237  "RegularImplicitSchurFactor::clone non implemented");
238  }
239 
240  bool empty() const override {
241  return false;
242  }
243 
245  return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
247  throw std::runtime_error(
248  "RegularImplicitSchurFactor::negate non implemented");
249  }
250 
251  // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
252  static
253  void multiplyHessianAdd(const Matrix& F, const Matrix& E,
254  const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
255  Vector e1 = F * x;
256  Vector d1 = E.transpose() * e1;
257  Vector d2 = PointCovariance * d1;
258  Vector e2 = E * d2;
259  Vector e3 = alpha * (e1 - e2);
260  y += F.transpose() * e3;
261  }
262 
263  typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
264 
268  void projectError2(const Error2s& e1, Error2s& e2) const {
269 
270  // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
271  Vector3 d1;
272  d1.setZero();
273  for (size_t k = 0; k < size(); k++)
274  d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
275  * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
276 
277  // d2 = E.transpose() * e1 = (3*2m)*2m
278  Vector3 d2 = PointCovariance_ * d1;
279 
280  // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
281  for (size_t k = 0; k < size(); k++)
282  e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
283  - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
284  }
285 
286  /*
287  * This definition matches the linearized error in the Hessian Factor:
288  * LinError(x) = x'*H*x - 2*x'*eta + f
289  * with:
290  * H = F' * (I-E'*P*E) * F = F' * Q * F
291  * eta = F' * (I-E'*P*E) * b = F' * Q * b
292  * f = nonlinear error
293  * (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
294  */
295  double error(const VectorValues& x) const override {
296 
297  // resize does not do malloc if correct size
298  e1.resize(size());
299  e2.resize(size());
300 
301  // e1 = F * x - b = (2m*dm)*dm
302  for (size_t k = 0; k < size(); ++k)
303  e1[k] = FBlocks_[k] * x.at(keys_[k]);
304  projectError2(e1, e2);
305 
306  double result = 0;
307  for (size_t k = 0; k < size(); ++k)
308  result += dot(e1[k], e2[k]);
309 
310  double f = b_.squaredNorm();
311  return 0.5 * (result + f);
312  }
313 
314  // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
315  // This is wrong and does not match the definition in Hessian,
316  // but it matches the definition of the Jacobian factor (JF)
317  double errorJF(const VectorValues& x) const {
318 
319  // resize does not do malloc if correct size
320  e1.resize(size());
321  e2.resize(size());
322 
323  // e1 = F * x - b = (2m*dm)*dm
324  for (size_t k = 0; k < size(); ++k)
325  e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
326  projectError(e1, e2);
327 
328  double result = 0;
329  for (size_t k = 0; k < size(); ++k)
330  result += dot(e2[k], e2[k]);
331 
332  // std::cout << "implicitFactor::error result " << result << std::endl;
333  return 0.5 * result;
334  }
338  void projectError(const Error2s& e1, Error2s& e2) const {
339 
340  // d1 = E.transpose() * e1 = (3*2m)*2m
341  Vector3 d1;
342  d1.setZero();
343  for (size_t k = 0; k < size(); k++)
344  d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
345 
346  // d2 = E.transpose() * e1 = (3*2m)*2m
347  Vector3 d2 = PointCovariance_ * d1;
348 
349  // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
350  for (size_t k = 0; k < size(); k++)
351  e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
352  }
353 
355  mutable Error2s e1, e2;
356 
361  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
362 
363  // Use eigen magic to access raw memory
364  typedef Eigen::Matrix<double, D, 1> DVector;
365  typedef Eigen::Map<DVector> DMap;
366  typedef Eigen::Map<const DVector> ConstDMap;
367 
368  // resize does not do malloc if correct size
369  e1.resize(size());
370  e2.resize(size());
371 
372  // e1 = F * x = (2m*dm)*dm
373  for (size_t k = 0; k < size(); ++k) {
374  Key key = keys_[k];
375  e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
376  }
377 
378  projectError(e1, e2);
379 
380  // y += F.transpose()*e2 = (2d*2m)*2m
381  for (size_t k = 0; k < size(); ++k) {
382  Key key = keys_[k];
383  DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
384  }
385  }
386 
387  void multiplyHessianAdd(double alpha, const double* x, double* y,
388  std::vector<size_t> keys) const {
389  }
390 
394  void multiplyHessianAdd(double alpha, const VectorValues& x,
395  VectorValues& y) const override {
396 
397  // resize does not do malloc if correct size
398  e1.resize(size());
399  e2.resize(size());
400 
401  // e1 = F * x = (2m*dm)*dm
402  for (size_t k = 0; k < size(); ++k)
403  e1[k] = FBlocks_[k] * x.at(keys_[k]);
404 
405  projectError(e1, e2);
406 
407  // y += F.transpose()*e2 = (2d*2m)*2m
408  for (size_t k = 0; k < size(); ++k) {
409  Key key = keys_[k];
410  static const Vector empty;
411  std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
412  Vector& yi = it.first->second;
413  // Create the value as a zero vector if it does not exist.
414  if (it.second)
415  yi = Vector::Zero(FBlocks_[k].cols());
416  yi += FBlocks_[k].transpose() * alpha * e2[k];
417  }
418  }
419 
423  void multiplyHessianDummy(double alpha, const VectorValues& x,
424  VectorValues& y) const {
425 
426  for (size_t k = 0; k < size(); ++k) {
427  static const Vector empty;
428  Key key = keys_[k];
429  std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
430  Vector& yi = it.first->second;
431  yi = x.at(key);
432  }
433  }
434 
438  VectorValues gradientAtZero() const override {
439  // calculate Q*b
440  e1.resize(size());
441  e2.resize(size());
442  for (size_t k = 0; k < size(); k++)
443  e1[k] = b_.segment<ZDim>(ZDim * k);
444  projectError(e1, e2);
445 
446  // g = F.transpose()*e2
447  VectorValues g;
448  for (size_t k = 0; k < size(); ++k) {
449  Key key = keys_[k];
450  g.insert(key, -FBlocks_[k].transpose() * e2[k]);
451  }
452 
453  // return it
454  return g;
455  }
456 
460  void gradientAtZero(double* d) const override {
461 
462  // Use eigen magic to access raw memory
463  typedef Eigen::Matrix<double, D, 1> DVector;
464  typedef Eigen::Map<DVector> DMap;
465 
466  // calculate Q*b
467  e1.resize(size());
468  e2.resize(size());
469  for (size_t k = 0; k < size(); k++)
470  e1[k] = b_.segment<ZDim>(ZDim * k);
471  projectError(e1, e2);
472 
473  for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
474  Key j = keys_[k];
475  DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
476  }
477  }
478 
480  Vector gradient(Key key, const VectorValues& x) const override {
481  throw std::runtime_error(
482  "gradient for RegularImplicitSchurFactor is not implemented yet");
483  }
484 
485 };
486 // end class RegularImplicitSchurFactor
487 
488 template<class CAMERA>
490 
491 template<class CAMERA>
493 
494 // traits
495 template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
496  RegularImplicitSchurFactor<CAMERA> > {
497 };
498 
499 }
500 
gtsam::Factor::print
void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::RegularImplicitSchurFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
Definition: RegularImplicitSchurFactor.h:111
JacobianFactor.h
gtsam::RegularImplicitSchurFactor::projectError2
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
Definition: RegularImplicitSchurFactor.h:268
gtsam::RegularImplicitSchurFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
Definition: RegularImplicitSchurFactor.h:182
gtsam::RegularImplicitSchurFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
Definition: RegularImplicitSchurFactor.h:151
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition: VectorValues.h:183
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:84
gtsam::RegularImplicitSchurFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
Definition: RegularImplicitSchurFactor.h:480
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
gtsam::RegularImplicitSchurFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: RegularImplicitSchurFactor.h:29
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::RegularImplicitSchurFactor::empty
bool empty() const override
Test whether the factor is empty.
Definition: RegularImplicitSchurFactor.h:240
gtsam::RegularImplicitSchurFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
Definition: RegularImplicitSchurFactor.h:120
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix &P, const Vector &b)
Construct from blocks of F, E, inv(E'*E), and RHS vector b.
Definition: RegularImplicitSchurFactor.h:55
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
gtsam::RegularImplicitSchurFactor::projectError
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition: RegularImplicitSchurFactor.h:338
VectorValues.h
Factor Graph Values.
gtsam::RegularImplicitSchurFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: RegularImplicitSchurFactor.h:208
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition: VectorValues.cpp:90
gtsam::RegularImplicitSchurFactor::multiplyHessianDummy
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
Definition: RegularImplicitSchurFactor.h:423
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::RegularImplicitSchurFactor::PointCovariance_
const Matrix PointCovariance_
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition: RegularImplicitSchurFactor.h:44
gtsam::RegularImplicitSchurFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
Definition: RegularImplicitSchurFactor.h:125
CameraSet.h
Base class to create smart factors on poses or cameras.
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition: RegularImplicitSchurFactor.h:361
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e.
Definition: RegularImplicitSchurFactor.h:394
gtsam::VectorValues::tryInsert
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
insert that mimics the STL map insert - if the value already exists, the map is not modified and an i...
Definition: VectorValues.h:207
gtsam::RegularImplicitSchurFactor::information
Matrix information() const override
Compute full information matrix
Definition: RegularImplicitSchurFactor.h:140
gtsam::RegularImplicitSchurFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RegularImplicitSchurFactor.h:82
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::RegularImplicitSchurFactor::augmentedInformation
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition: RegularImplicitSchurFactor.h:132
gtsam::RegularImplicitSchurFactor::E_
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Definition: RegularImplicitSchurFactor.h:45
gtsam::RegularImplicitSchurFactor::equals
bool equals(const GaussianFactor &lf, double tol) const override
equals
Definition: RegularImplicitSchurFactor.h:95
gtsam::RegularImplicitSchurFactor
RegularImplicitSchurFactor.
Definition: RegularImplicitSchurFactor.h:25
gtsam::RegularImplicitSchurFactor::MatrixDD
Eigen::Matrix< double, D, D > MatrixDD
camera hessian
Definition: RegularImplicitSchurFactor.h:41
gtsam::RegularImplicitSchurFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition: RegularImplicitSchurFactor.h:115
gtsam::RegularImplicitSchurFactor::~RegularImplicitSchurFactor
virtual ~RegularImplicitSchurFactor()
Destructor.
Definition: RegularImplicitSchurFactor.h:62
gtsam::RegularImplicitSchurFactor::e1
Error2s e1
Scratch space for multiplyHessianAdd.
Definition: RegularImplicitSchurFactor.h:355
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor()
Constructor.
Definition: RegularImplicitSchurFactor.h:51
gtsam::RegularImplicitSchurFactor::error
double error(const VectorValues &x) const override
Print for testable.
Definition: RegularImplicitSchurFactor.h:295
gtsam::RegularImplicitSchurFactor::FBlocks_
const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks_
All ZDim*D F blocks (one for each camera)
Definition: RegularImplicitSchurFactor.h:43
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::RegularImplicitSchurFactor::b_
const Vector b_
2m-dimensional RHS vector
Definition: RegularImplicitSchurFactor.h:46
gtsam::KeyFormatter
boost::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
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::RegularImplicitSchurFactor::D
static const int D
Camera dimension.
Definition: RegularImplicitSchurFactor.h:37
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:161
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: GaussianFactor.cpp:27
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::RegularImplicitSchurFactor::gradientAtZero
void gradientAtZero(double *d) const override
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Definition: RegularImplicitSchurFactor.h:460
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::RegularImplicitSchurFactor::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Definition: RegularImplicitSchurFactor.h:40
gtsam::RegularImplicitSchurFactor::gradientAtZero
VectorValues gradientAtZero() const override
Calculate gradient, which is -F'Q*b, see paper.
Definition: RegularImplicitSchurFactor.h:438
gtsam::RegularImplicitSchurFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone a factor (make a deep copy)
Definition: RegularImplicitSchurFactor.h:233
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition: CameraSet.h:149
gtsam::RegularImplicitSchurFactor::ZDim
static const int ZDim
Measurement dimension.
Definition: RegularImplicitSchurFactor.h:38
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
gtsam::RegularImplicitSchurFactor::negate
GaussianFactor::shared_ptr negate() const override
Construct the corresponding anti-factor to negate information stored stored in this factor.
Definition: RegularImplicitSchurFactor.h:244
gtsam::RegularImplicitSchurFactor::This
RegularImplicitSchurFactor This
Typedef to this class.
Definition: RegularImplicitSchurFactor.h:28
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition: VectorValues.h:137