Loading...
Searching...
No Matches
BinaryJacobianFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/timing.h>
namespace gtsam {
template<int M, int N1, int N2>
struct BinaryJacobianFactor: JacobianFactor {
BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
Key key2, const Eigen::Matrix<double, M, N2>& A2,
const Eigen::Matrix<double, M, 1>& b, //
const SharedDiagonal& model = SharedDiagonal()) :
JacobianFactor(key1, A1, key2, A2, b, model) {
}
inline Key key1() const {
return keys_[0];
}
inline Key key2() const {
return keys_[1];
}
// Fixed-size matrix update
void updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const override {
gttic(updateHessian_BinaryJacobianFactor);
// Whiten the factor if it has a noise model
const SharedDiagonal& model = get_model();
if (model && !model->isUnit()) {
if (model->isConstrained())
throw std::invalid_argument(
"BinaryJacobianFactor::updateHessian: cannot update information with "
"constrained noise model");
BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
key2(), model->Whiten(getA(end())), model->whiten(getb()));
whitenedFactor.updateHessian(infoKeys, info);
} else {
// First build an array of slots
DenseIndex slot1 = Slot(infoKeys, key1());
DenseIndex slot2 = Slot(infoKeys, key2());
DenseIndex slotB = info->nBlocks() - 1;
const Matrix& Ab = Ab_.matrix();
Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
// We perform I += A'*A to the upper triangle
info->diagonalBlock(slot1).rankUpdate(A1.transpose());
info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2);
info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b);
info->diagonalBlock(slot2).rankUpdate(A2.transpose());
info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b);
info->updateDiagonalBlock(slotB, b.transpose() * b);
}
}
};
template<int M, int N1, int N2>
struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
BinaryJacobianFactor<M, N1, N2> > {
};
} //namespace gtsam
timing.h
Timing utilities.
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
JacobianFactor.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
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::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition Factor.h:146
gtsam::BinaryJacobianFactor
A binary JacobianFactor specialization that uses fixed matrix math for speed.
Definition BinaryJacobianFactor.h:33
gtsam::BinaryJacobianFactor::BinaryJacobianFactor
BinaryJacobianFactor(Key key1, const Eigen::Matrix< double, M, N1 > &A1, Key key2, const Eigen::Matrix< double, M, N2 > &A2, const Eigen::Matrix< double, M, 1 > &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition BinaryJacobianFactor.h:36
gtsam::BinaryJacobianFactor::updateHessian
void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition BinaryJacobianFactor.h:51
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const GaussianFactor &gf)
Convert from other GaussianFactor.
Definition JacobianFactor.cpp:60
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition JacobianFactor.h:300