JacobianFactorQ.h
Go to the documentation of this file.
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 
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 }
Matrix3f m
Eigen::Matrix< double, ZDim, D > MatrixZD
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Key E(std::uint64_t j)
noiseModel::Diagonal::shared_ptr model
MatrixType m2(n_dims)
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
const vector< Matrix26, Eigen::aligned_allocator< Matrix26 > > FBlocks
RegularJacobianFactor< D > Base
JacobianFactorQ()
Default constructor.
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.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Eigen::VectorXd Vector
Definition: Vector.h:38
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:721
JacobianFactor class with fixed sized blcoks.
const G & b
Definition: Group.h:83
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
traits
Definition: chartTesting.h:28
std::pair< Key, Matrix > KeyMatrix
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:124
The quaternion class used to represent 3D orientations and rotations.
JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
The matrix class, also used for vectors and row-vectors.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:23