gtsam 4.1.1 gtsam
BinaryJacobianFactor.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
21#pragma once
22
25#include <gtsam/base/timing.h>
26
27namespace gtsam {
28
32template<int M, int N1, int N2>
34
36 BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
37 Key key2, const Eigen::Matrix<double, M, N2>& A2,
38 const Eigen::Matrix<double, M, 1>& b, //
39 const SharedDiagonal& model = SharedDiagonal()) :
40 JacobianFactor(key1, A1, key2, A2, b, model) {
41 }
42
43 inline Key key1() const {
44 return keys_[0];
45 }
46 inline Key key2() const {
47 return keys_[1];
48 }
49
50 // Fixed-size matrix update
51 void updateHessian(const KeyVector& infoKeys,
52 SymmetricBlockMatrix* info) const override {
53 gttic(updateHessian_BinaryJacobianFactor);
54 // Whiten the factor if it has a noise model
55 const SharedDiagonal& model = get_model();
56 if (model && !model->isUnit()) {
57 if (model->isConstrained())
58 throw std::invalid_argument(
59 "BinaryJacobianFactor::updateHessian: cannot update information with "
60 "constrained noise model");
61 BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
62 key2(), model->Whiten(getA(end())), model->whiten(getb()));
63 whitenedFactor.updateHessian(infoKeys, info);
64 } else {
65 // First build an array of slots
66 DenseIndex slot1 = Slot(infoKeys, key1());
67 DenseIndex slot2 = Slot(infoKeys, key2());
68 DenseIndex slotB = info->nBlocks() - 1;
69
70 const Matrix& Ab = Ab_.matrix();
71 Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
72 Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
73 Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
74
75 // We perform I += A'*A to the upper triangle
76 info->diagonalBlock(slot1).rankUpdate(A1.transpose());
77 info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2);
78 info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b);
79 info->diagonalBlock(slot2).rankUpdate(A2.transpose());
80 info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b);
81 info->updateDiagonalBlock(slotB, b.transpose() * b);
82 }
83 }
84};
85
86template<int M, int N1, int N2>
87struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
88 BinaryJacobianFactor<M, N1, N2> > {
89};
90
91} //namespace gtsam
Timing utilities.
Access to matrices via blocks of pre-defined sizes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:75
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Definition: SymmetricBlockMatrix.h:52
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition: SymmetricBlockMatrix.h:233
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
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:217
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:125
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition: VerticalBlockMatrix.h:187
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:73
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:128
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:131
A binary JacobianFactor specialization that uses fixed matrix math for speed.
Definition: BinaryJacobianFactor.h:33
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
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
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
const SharedDiagonal & get_model() const
get a copy of model
Definition: JacobianFactor.h:289
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition: JacobianFactor.h:301