GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#include <JacobianFactor.h>
Public Types | |
typedef JacobianFactor | This |
Typedef to this class. | |
typedef GaussianFactor | Base |
Typedef to base class. | |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class | |
typedef VerticalBlockMatrix::Block | ABlock |
typedef VerticalBlockMatrix::constBlock | constABlock |
typedef ABlock::ColXpr | BVector |
typedef constABlock::ConstColXpr | constBVector |
typedef KeyVector::iterator | iterator |
Iterator over keys. | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. | |
Public Member Functions | |
JacobianFactor (const GaussianFactor &gf) | |
JacobianFactor (const JacobianFactor &jf) | |
JacobianFactor (const HessianFactor &hf) | |
JacobianFactor () | |
JacobianFactor (const Vector &b_in) | |
JacobianFactor (Key i1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
template<typename TERMS > | |
JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
template<typename KEYS > | |
JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) | |
JacobianFactor (const GaussianFactorGraph &graph) | |
JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots) | |
~JacobianFactor () override | |
GaussianFactor::shared_ptr | clone () const override |
void | print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print with optional string | |
bool | equals (const GaussianFactor &lf, double tol=1e-9) const override |
assert equality up to a tolerance | |
Vector | unweighted_error (const VectorValues &c) const |
Vector | error_vector (const VectorValues &c) const |
double | error (const VectorValues &c) const override |
Matrix | augmentedInformation () const override |
Matrix | information () const override |
void | hessianDiagonalAdd (VectorValues &d) const override |
Add the current diagonal to a VectorValues instance. | |
void | hessianDiagonal (double *d) const override |
Raw memory access version of hessianDiagonal. | |
std::map< Key, Matrix > | hessianBlockDiagonal () const override |
Return the block diagonal of the Hessian for this factor. | |
std::pair< Matrix, Vector > | jacobian () const override |
Returns (dense) A,b pair associated with factor, bakes in the weights. | |
std::pair< Matrix, Vector > | jacobianUnweighted () const |
Returns (dense) A,b pair associated with factor, does not bake in weights. | |
Matrix | augmentedJacobian () const override |
Matrix | augmentedJacobianUnweighted () const |
const VerticalBlockMatrix & | matrixObject () const |
VerticalBlockMatrix & | matrixObject () |
GaussianFactor::shared_ptr | negate () const override |
bool | isConstrained () const |
DenseIndex | getDim (const_iterator variable) const override |
size_t | rows () const |
size_t | cols () const |
const SharedDiagonal & | get_model () const |
SharedDiagonal & | get_model () |
const constBVector | getb () const |
constABlock | getA (const_iterator variable) const |
constABlock | getA () const |
BVector | getb () |
ABlock | getA (iterator variable) |
ABlock | getA () |
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
Vector | operator* (const VectorValues &x) const |
void | transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const |
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override |
void | multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const |
VectorValues | gradientAtZero () const override |
A'*b for Jacobian. | |
void | gradientAtZero (double *d) const override |
A'*b for Jacobian (raw memory version) | |
Vector | gradient (Key key, const VectorValues &x) const override |
Compute the gradient wrt a key at any values. | |
JacobianFactor | whiten () const |
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | eliminate (const Ordering &keys) |
void | setModel (bool anyConstrained, const Vector &sigmas) |
std::shared_ptr< GaussianConditional > | splitConditional (size_t nrFrontals) |
Testable | |
bool | equals (const This &other, double tol=1e-9) const |
check equality | |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys | |
Standard Interface | |
double | error (const HybridValues &c) const override |
VectorValues | hessianDiagonal () const |
Return the diagonal of the Hessian for this factor. | |
Standard Interface | |
bool | empty () const |
Whether the factor is empty (involves zero variables). | |
Key | front () const |
First key. | |
Key | back () const |
Last key. | |
const_iterator | find (Key key) const |
find | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
Advanced Interface | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Static Public Member Functions | |
Advanced Interface | |
template<typename CONTAINER > | |
static DenseIndex | Slot (const CONTAINER &keys, Key key) |
Protected Member Functions | |
template<typename TERMS > | |
void | fillTerms (const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel) |
Internal function to fill blocks and set dimensions. | |
Static Protected Member Functions | |
Standard Constructors | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
Protected Attributes | |
VerticalBlockMatrix | Ab_ |
noiseModel::Diagonal::shared_ptr | model_ |
KeyVector | keys_ |
The keys involved in this factor. | |
Friends | |
template<typename T > | |
class | ExpressionFactor |
GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys) |
A Gaussian factor in the squared-error form.
JacobianFactor implements a Gaussian, which has quadratic negative log-likelihood
\[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \]
where \( \Sigma \) is a diagonal covariance matrix. The matrix \( A \), r.h.s. vector \( b \), and diagonal noise model \( \Sigma \) are stored in this class.
This factor represents the sum-of-squares error of a linear measurement function, and is created upon linearization of a NoiseModelFactor, which in turn is a sum-of-squares factor with a nonlinear measurement function.
Here is an example of how this factor represents a sum-of-squares error:
Letting \( h(x) \) be a linear measurement prediction function, \( z \) be the actual observed measurement, the residual is
\[ f(x) = h(x) - z . \]
If we expect noise with diagonal covariance matrix \( \Sigma \) on this measurement, then the negative log-likelihood of the Gaussian induced by this measurement model is
\[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \]
Because \( h(x) \) is linear, we can write it as
\[ h(x) = Ax + e \]
and thus we have
\[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \]
where \( b = z - e \).
This factor can involve an arbitrary number of variables, and in the above example \( x \) would almost always be only be a subset of the variables in the entire factor graph. There are special constructors for 1-, 2-, and 3- way JacobianFactors, and additional constructors for creating n-way JacobianFactors. The Jacobian matrix \( A \) is passed to these constructors in blocks, for example, for a 2-way factor, the constructor would accept \( A1 \) and \( A2 \), as well as the variable indices \( j1 \) and \( j2 \) and the negative log-likelihood represented by this factor would be
\[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \]
HessianFactor, which represent a Gaussian likelihood over a set of variables.
|
explicit |
Convert from other GaussianFactor
|
inline |
Copy constructor
|
explicit |
Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)
gtsam::JacobianFactor::JacobianFactor | ( | ) |
default constructor for I/O
|
explicit |
Construct Null factor
gtsam::JacobianFactor::JacobianFactor | ( | Key | i1, |
const Matrix & | A1, | ||
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct unary factor
gtsam::JacobianFactor::JacobianFactor | ( | Key | i1, |
const Matrix & | A1, | ||
Key | i2, | ||
const Matrix & | A2, | ||
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct binary factor
gtsam::JacobianFactor::JacobianFactor | ( | Key | i1, |
const Matrix & | A1, | ||
Key | i2, | ||
const Matrix & | A2, | ||
Key | i3, | ||
const Matrix & | A3, | ||
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct ternary factor
JacobianFactor::JacobianFactor | ( | const TERMS & | terms, |
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct an n-ary factor
TERMS | A container whose value type is std::pair<Key, Matrix>, specifying the collection of keys and matrices making up the factor. |
JacobianFactor::JacobianFactor | ( | const KEYS & | keys, |
const VerticalBlockMatrix & | augmentedMatrix, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead of in block terms. Note that only the active view of the provided augmented matrix is used, and that the matrix data is copied into a newly-allocated matrix in the constructed factor.
|
explicit |
Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph
is already available, providing it will reduce the amount of computation performed.
|
explicit |
Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph
is already available, providing it will reduce the amount of computation performed.
|
explicit |
Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph
is already available, providing it will reduce the amount of computation performed.
|
explicit |
Build a dense joint factor from all the factors in a factor graph. If a VariableSlots structure computed for graph
is already available, providing it will reduce the amount of computation performed.
|
inlineoverride |
Virtual destructor
|
overridevirtual |
Return the augmented information matrix represented by this GaussianFactor. The augmented information matrix contains the information matrix with an additional column holding the information vector, and an additional row holding the transpose of the information vector. The lower-right entry contains the constant error term (when \( \delta x = 0 \)). The augmented information matrix is described in more detail in HessianFactor, which in fact stores an augmented information matrix.
Implements gtsam::GaussianFactor.
|
overridevirtual |
Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are baked in
Implements gtsam::GaussianFactor.
Matrix gtsam::JacobianFactor::augmentedJacobianUnweighted | ( | ) | const |
Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are not baked in
|
inlineinherited |
Iterator at beginning of involved variable keys
|
inlineinherited |
Iterator at beginning of involved variable keys
|
inlineoverridevirtual |
Clone this JacobianFactor
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
|
inline |
return the number of columns in the corresponding linear system
std::pair<std::shared_ptr<GaussianConditional>, shared_ptr> gtsam::JacobianFactor::eliminate | ( | const Ordering & | keys | ) |
Eliminate the requested variables.
|
inlineinherited |
Iterator at end of involved variable keys
|
inlineinherited |
Iterator at end of involved variable keys
|
overridevirtualinherited |
All factor types need to implement an error function. In factor graphs, this is the negative log-likelihood.
Reimplemented from gtsam::Factor.
Vector gtsam::JacobianFactor::error_vector | ( | const VectorValues & | c | ) | const |
(A*x-b)
|
inlinestaticprotectedinherited |
Construct factor from iterator keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inlinestaticprotectedinherited |
Construct factor from container of keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inline |
get a copy of model
|
inline |
get a copy of model (non-const version)
|
inline |
Get a view of the A matrix for the variable pointed to by the given key iterator
|
inline |
Get a view of the A matrix, not weighted by noise
|
inline |
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
|
inline |
Get a view of the A matrix
|
inline |
Get a view of the r.h.s. vector b, not weighted by noise
|
inline |
Get a view of the r.h.s. vector b (non-const version)
|
inlineoverridevirtual |
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor of keeping track of dimensions with variables?
Implements gtsam::GaussianFactor.
|
overridevirtual |
Return the non-augmented information matrix represented by this GaussianFactor.
Implements gtsam::GaussianFactor.
|
inline |
is noise model constrained ?
|
inlineinherited |
|
inline |
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
|
inline |
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
|
overridevirtual |
y += alpha * A'*A*x
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularJacobianFactor< D >.
void gtsam::JacobianFactor::multiplyHessianAdd | ( | double | alpha, |
const double * | x, | ||
double * | y, | ||
const std::vector< size_t > & | accumulatedDims | ||
) | const |
Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x Requires the vector accumulatedDims to tell the dimension of each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, then accumulatedDims is [0 3 9 11 13] NOTE: size of accumulatedDims is size of keys + 1!! TODO(frank): we should probably kill this if no longer needed
|
overridevirtual |
Construct the corresponding anti-factor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.
Vector gtsam::JacobianFactor::operator* | ( | const VectorValues & | x | ) | const |
Return A*x
|
inline |
return the number of rows in the corresponding linear system
void gtsam::JacobianFactor::setModel | ( | bool | anyConstrained, |
const Vector & | sigmas | ||
) |
set noiseModel correctly
|
inlineinherited |
std::shared_ptr<GaussianConditional> gtsam::JacobianFactor::splitConditional | ( | size_t | nrFrontals | ) |
splits a pre-factorized factor into a conditional, and changes the current factor to be the remaining component. Performs same operation as eliminate(), but without running QR. NOTE: looks at dimension of noise model to determine how many rows to keep.
nrFrontals | number of keys to eliminate |
void gtsam::JacobianFactor::transposeMultiplyAdd | ( | double | alpha, |
const Vector & | e, | ||
VectorValues & | x | ||
) | const |
x += alpha * A'*e. If x is initially missing any values, they are created and assumed to start as zero vectors.
|
overridevirtual |
Update an information matrix by adding the information corresponding to this factor (used internally during elimination).
scatter | A mapping from variable index to slot index in this HessianFactor |
info | The information matrix to be updated |
Implements gtsam::GaussianFactor.
JacobianFactor gtsam::JacobianFactor::whiten | ( | ) | const |
Return a whitened version of the factor, i.e. with unit diagonal noise model.