JacobianFactor-inl.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 
19 #pragma once
20 
22 
23 namespace gtsam {
24 
25  /* ************************************************************************* */
26  template<typename TERMS>
27  JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
28  {
29  fillTerms(terms, b, model);
30  }
31 
32  /* ************************************************************************* */
33  template<typename KEYS>
35  const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
36  Base(keys), Ab_(augmentedMatrix)
37  {
38  // Check noise model dimension
39  if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
40  throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
41 
42  // Check number of variables
43  if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
44  throw std::invalid_argument(
45  "Error in JacobianFactor constructor input. Number of provided keys plus\n"
46  "one for the RHS vector must equal the number of provided matrix blocks.");
47 
48  // Check RHS dimension
49  if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
50  throw std::invalid_argument(
51  "Error in JacobianFactor constructor input. The last provided matrix block\n"
52  "must be the RHS vector, but the last provided block had more than one column.");
53 
54  // Take noise model
55  model_ = model;
56  }
57 
58  /* ************************************************************************* */
59  template<typename TERMS>
60  void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
61  {
62  // Check noise model dimension
63  if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
64  throw InvalidNoiseModel(b.size(), noiseModel->dim());
65 
66  // Resize base class key vector
67  Base::keys_.resize(terms.size());
68 
69  // Get dimensions of matrices
70  std::vector<size_t> dimensions;
71  dimensions.reserve(terms.size());
72  for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
73  const std::pair<Key, Matrix>& term = *it;
74  const Matrix& Ai = term.second;
75  dimensions.push_back(Ai.cols());
76  }
77 
78  // Construct block matrix
79  Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
80 
81  // Check and add terms
82  DenseIndex i = 0; // For block index
83  for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
84  const std::pair<Key, Matrix>& term = *it;
85  Key key = term.first;
86  const Matrix& Ai = term.second;
87 
88  // Check block rows
89  if(Ai.rows() != Ab_.rows())
90  throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
91 
92  // Assign key and matrix
93  Base::keys_[i] = key;
94  Ab_(i) = Ai;
95 
96  // Increment block index
97  ++ i;
98  }
99 
100  // Assign RHS vector
101  getb() = b;
102 
103  // Assign noise model
104  model_ = noiseModel;
105  }
106 
107 } // gtsam
108 
const gtsam::Symbol key('X', 0)
DenseIndex nBlocks() const
Block count.
noiseModel::Diagonal::shared_ptr model
DenseIndex rows() const
Row size.
noiseModel::Diagonal::shared_ptr model_
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:87
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
Eigen::VectorXd Vector
Definition: Vector.h:38
const constBVector getb() const
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
const G & b
Definition: Group.h:86
Exceptions that may be thrown by linear solver components.
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
VerticalBlockMatrix Ab_
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
const std::vector< size_t > dimensions
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:27