gtsam  4.0.0
gtsam
JacobianFactorSVD.h
1 /*
2  * @file JacobianFactorSVD.h
3  * @date Oct 27, 2013
4  * @uthor Frank Dellaert
5  */
6 
7 #pragma once
9 
10 namespace gtsam {
27 template<size_t D, size_t ZDim>
29 
31  typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
32  typedef std::pair<Key, Matrix> KeyMatrix;
33 
34 public:
35 
38  }
39 
42  const SharedDiagonal& model = SharedDiagonal()) :
43  Base() {
44  Matrix zeroMatrix = Matrix::Zero(0, D);
45  Vector zeroVector = Vector::Zero(0);
46  std::vector<KeyMatrix> QF;
47  QF.reserve(keys.size());
48  for(const Key& key: keys)
49  QF.push_back(KeyMatrix(key, zeroMatrix));
50  JacobianFactor::fillTerms(QF, zeroVector, model);
51  }
52 
62  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
63  const Vector& b, //
64  const SharedDiagonal& model = SharedDiagonal()) :
65  Base() {
66  size_t numKeys = Enull.rows() / ZDim;
67  size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
68  // PLAIN NULL SPACE TRICK
69  // Matrix Q = Enull * Enull.transpose();
70  // for(const KeyMatrixZD& it: Fblocks)
71  // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
72  // JacobianFactor factor(QF, Q * b);
73  std::vector<KeyMatrix> QF;
74  QF.reserve(numKeys);
75  for (size_t k = 0; k < Fblocks.size(); ++k) {
76  Key key = keys[k];
77  QF.push_back(
78  KeyMatrix(key,
79  (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
80  }
81  JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
82  }
83 };
84 
85 }
This is the base class for all factor types.
Definition: Factor.h:54
JacobianFactor class with fixed sized blcoks.
JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis.
Definition: JacobianFactorSVD.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) and a reduced poin...
Definition: JacobianFactorSVD.h:61
JacobianFactorSVD()
Default constructor.
Definition: JacobianFactorSVD.h:37
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition: JacobianFactor-inl.h:60
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition: JacobianFactorSVD.h:41
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32