gtsam  4.1.0
gtsam
JacobianFactor-inl.h
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 
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
gtsam::InvalidMatrixBlock
An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
Definition: linearExceptions.h:124
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: VerticalBlockMatrix.h:120
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
linearExceptions.h
Exceptions that may be thrown by linear solver components.
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition: JacobianFactor-inl.h:60
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::InvalidNoiseModel
An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
Definition: linearExceptions.h:106
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:117
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
default constructor for I/O
Definition: JacobianFactor.cpp:60
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42