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
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 }
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
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::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::JacobianFactorQ::Base
RegularJacobianFactor< D > Base
Definition: JacobianFactorQ.h:29
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition: JacobianFactorQ.h:40
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)
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
gtsam::JacobianFactorQ::KeyMatrix
std::pair< Key, Matrix > KeyMatrix
Definition: JacobianFactorQ.h:31
FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:222
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
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::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ()
Default constructor.
Definition: JacobianFactorQ.h:36
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::JacobianFactorQ::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definition: JacobianFactorQ.h:30
key
const gtsam::Symbol key('X', 0)
E
DiscreteKey E(5, 2)
gtsam::b
const G & b
Definition: Group.h:79
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
P
static double P[]
Definition: ellpe.c:68
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::JacobianFactorQ
Definition: JacobianFactorQ.h:27
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


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