BinaryJacobianFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
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 
27 namespace gtsam {
28 
32 template<int M, int N1, int N2>
34 
39  const SharedDiagonal& model = SharedDiagonal()) :
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();
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 
86 template<int M, int N1, int N2>
87 struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
88  BinaryJacobianFactor<M, N1, N2> > {
89 };
90 
91 } //namespace gtsam
timing.h
Timing utilities.
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
gtsam::JacobianFactor::Ab_
VerticalBlockMatrix Ab_
Definition: JacobianFactor.h:106
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::BinaryJacobianFactor::updateHessian
void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const override
Definition: BinaryJacobianFactor.h:51
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor::begin
const_iterator begin() const
Definition: Factor.h:146
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::BinaryJacobianFactor
Definition: BinaryJacobianFactor.h:33
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
gtsam::Factor::end
const_iterator end() const
Definition: Factor.h:149
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
JacobianFactor.h
gtsam::b
const G & b
Definition: Group.h:79
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
gtsam::BinaryJacobianFactor::key1
Key key1() const
Definition: BinaryJacobianFactor.h:43
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Definition: VerticalBlockMatrix.h:188
A1
static const double A1[]
Definition: expn.h:6
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
gtsam::GaussianFactor::Slot
static DenseIndex Slot(const CONTAINER &keys, Key key)
Definition: GaussianFactor.h:175
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::BinaryJacobianFactor::key2
Key key2() const
Definition: BinaryJacobianFactor.h:46
gtsam::JacobianFactor::getA
constABlock getA() const
Definition: JacobianFactor.h:304
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gttic
#define gttic(label)
Definition: timing.h:295
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
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:02