JacobianFactorSVD.h
Go to the documentation of this file.
1 /*
2  * @file JacobianFactorSVD.h
3  * @date Oct 27, 2013
4  * @uthor Frank Dellaert
5  */
6 
7 #pragma once
9 
10 namespace gtsam {
28 template<size_t D, size_t ZDim>
30 
32  typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
33  typedef std::pair<Key, Matrix> KeyMatrix;
34 
35 public:
36 
39  }
40 
44  : Base() {
45  Matrix zeroMatrix = Matrix::Zero(0, D);
46  Vector zeroVector = Vector::Zero(0);
47  std::vector<KeyMatrix> QF;
48  QF.reserve(keys.size());
49  for(const Key& key: keys)
50  QF.push_back(KeyMatrix(key, zeroMatrix));
51  JacobianFactor::fillTerms(QF, zeroVector, model);
52  }
53 
65  const KeyVector& keys,
66  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
67  const Matrix& Enull, const Vector& b,
69  : Base() {
70  size_t numKeys = Enull.rows() / ZDim;
71  size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
72  // PLAIN nullptr SPACE TRICK
73  // Matrix Q = Enull * Enull.transpose();
74  // for(const KeyMatrixZD& it: Fblocks)
75  // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
76  // JacobianFactor factor(QF, Q * b);
77  std::vector<KeyMatrix> QF;
78  QF.reserve(numKeys);
79  for (size_t k = 0; k < Fblocks.size(); ++k) {
80  Key key = keys[k];
81  QF.emplace_back(
82  key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
83  }
84  JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
85  }
86 };
87 
88 }
gtsam::JacobianFactorSVD
Definition: JacobianFactorSVD.h:29
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD()
Default constructor.
Definition: JacobianFactorSVD.h:38
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition: JacobianFactor-inl.h:60
m2
MatrixType m2(n_dims)
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition: JacobianFactorSVD.h:42
gtsam::JacobianFactorSVD::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definition: JacobianFactorSVD.h:32
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
ZDim
static const int ZDim
Measurement dimension (Point2)
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:218
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet.
Definition: JacobianFactorSVD.h:64
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
gtsam::b
const G & b
Definition: Group.h:79
gtsam::JacobianFactorSVD::Base
RegularJacobianFactor< D > Base
Definition: JacobianFactorSVD.h:31
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::JacobianFactorSVD::KeyMatrix
std::pair< Key, Matrix > KeyMatrix
Definition: JacobianFactorSVD.h:33


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:55