gtsam  4.1.0
gtsam
JacobianFactorQ.h
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 
12 /*
13  * @file JacobianFactorQ.h
14  * @date Oct 27, 2013
15  * @uthor Frank Dellaert
16  */
17 
18 #pragma once
19 
21 
22 namespace gtsam {
26 template<size_t D, size_t ZDim>
28 
30  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
31  typedef std::pair<Key, Matrix> KeyMatrix;
32 
33 public:
34 
37  }
38 
41  const SharedDiagonal& model = SharedDiagonal()) :
42  Base() {
43  Matrix zeroMatrix = Matrix::Zero(0, D);
44  Vector zeroVector = Vector::Zero(0);
45  std::vector<KeyMatrix> QF;
46  QF.reserve(keys.size());
47  for(const Key& key: keys)
48  QF.push_back(KeyMatrix(key, zeroMatrix));
49  JacobianFactor::fillTerms(QF, zeroVector, model);
50  }
51 
54  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
55  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
56  Base() {
57  size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
58  // Calculate projector Q
59  Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
60  // Calculate pre-computed Jacobian matrices
61  // TODO: can we do better ?
62  std::vector<KeyMatrix> QF;
63  QF.reserve(m);
64  // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
65  for (size_t k = 0; k < FBlocks.size(); ++k) {
66  Key key = keys[k];
67  QF.push_back(
68  KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
69  }
70  // Which is then passed to the normal JacobianFactor constructor
71  JacobianFactor::fillTerms(QF, - Q * b, model);
72  }
73 };
74 // end class JacobianFactorQ
75 
76 // traits
77 template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
78  JacobianFactorQ<D, ZDim> > {
79 };
80 
81 }
gtsam::JacobianFactorQ
JacobianFactor for Schur complement that uses Q noise model.
Definition: JacobianFactorQ.h:27
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
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
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ()
Default constructor.
Definition: JacobianFactorQ.h:36
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition: JacobianFactorQ.h:53
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition: JacobianFactorQ.h:40