Loading...
Searching...
No Matches
JacobianFactor-inl.h
/* ----------------------------------------------------------------------------
* 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/linearExceptions.h>
namespace gtsam {
/* ************************************************************************* */
template<typename TERMS>
JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
{
fillTerms(terms, b, model);
}
/* ************************************************************************* */
template<typename KEYS>
JacobianFactor::JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
Base(keys), Ab_(augmentedMatrix)
{
// Check noise model dimension
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables
if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension
if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n"
"must be the RHS vector, but the last provided block had more than one column.");
// Take noise model
model_ = model;
}
/* ************************************************************************* */
template<typename TERMS>
void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
{
// Check noise model dimension
if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
throw InvalidNoiseModel(b.size(), noiseModel->dim());
// Resize base class key vector
Base::keys_.resize(terms.size());
// Get dimensions of matrices
std::vector<size_t> dimensions;
dimensions.reserve(terms.size());
for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
const std::pair<Key, Matrix>& term = *it;
const Matrix& Ai = term.second;
dimensions.push_back(Ai.cols());
}
// Construct block matrix
Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
// Check and add terms
DenseIndex i = 0; // For block index
for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
const std::pair<Key, Matrix>& term = *it;
Key key = term.first;
const Matrix& Ai = term.second;
// Check block rows
if(Ai.rows() != Ab_.rows())
throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
// Assign key and matrix
Base::keys_[i] = key;
Ab_(i) = Ai;
// Increment block index
++ i;
}
// Assign RHS vector
getb() = b;
// Assign noise model
model_ = noiseModel;
}
} // gtsam
linearExceptions.h
Exceptions that may be thrown by linear solver components.
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
default constructor for I/O
Definition JacobianFactor.cpp:54
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::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition JacobianFactor.h:95
gtsam::InvalidNoiseModel
An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
Definition linearExceptions.h:106
gtsam::InvalidMatrixBlock
An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
Definition linearExceptions.h:124