82 lines
2.4 KiB
C++
82 lines
2.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 {
|
|
/**
|
|
* JacobianFactor for Schur complement that uses Q noise model
|
|
*/
|
|
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:
|
|
|
|
/// Default constructor
|
|
JacobianFactorQ() {
|
|
}
|
|
|
|
/// Empty constructor with keys
|
|
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);
|
|
}
|
|
|
|
/// 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()) :
|
|
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> > {
|
|
};
|
|
|
|
}
|