Loading...
Searching...
No Matches
JacobianFactorQ.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file JacobianFactorQ.h
* @date Oct 27, 2013
* @uthor Frank Dellaert
*/
#pragma once
#include <gtsam/linear/RegularJacobianFactor.h>
namespace gtsam {
template<size_t D, size_t ZDim>
class JacobianFactorQ: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
JacobianFactorQ() {
}
JacobianFactorQ(const KeyVector& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
for(const Key& key: keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
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()) :
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
// TODO: can we do better ?
std::vector<KeyMatrix> QF;
QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k];
QF.push_back(
KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
}
// Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, - Q * b, model);
}
};
// end class JacobianFactorQ
// traits
template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
JacobianFactorQ<D, ZDim> > {
};
}
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
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::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition RegularJacobianFactor.h:44
gtsam::JacobianFactorQ
JacobianFactor for Schur complement that uses Q noise model.
Definition JacobianFactorQ.h:27
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ()
Default constructor.
Definition JacobianFactorQ.h:36
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