Loading...
Searching...
No Matches
JacobianFactorSVD.h
/*
* @file JacobianFactorSVD.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 JacobianFactorSVD: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
typedef std::pair<Key, Matrix> KeyMatrix;
public:
JacobianFactorSVD() {
}
JacobianFactorSVD(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);
}
JacobianFactorSVD(
const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal())
: Base() {
size_t numKeys = Enull.rows() / ZDim;
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
// PLAIN nullptr SPACE TRICK
// Matrix Q = Enull * Enull.transpose();
// for(const KeyMatrixZD& it: Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
for (size_t k = 0; k < Fblocks.size(); ++k) {
Key key = keys[k];
QF.emplace_back(
key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
}
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
}
};
}
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::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::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD()
Default constructor.
Definition JacobianFactorSVD.h:38
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
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition JacobianFactorSVD.h:42