#include <JacobianFactor.h>
Public Types | |
typedef VerticalBlockMatrix::Block | ABlock |
typedef GaussianFactor | Base |
Typedef to base class. More... | |
typedef ABlock::ColXpr | BVector |
typedef VerticalBlockMatrix::constBlock | constABlock |
typedef constABlock::ConstColXpr | constBVector |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef JacobianFactor | This |
Typedef to this class. More... | |
Public Types inherited from gtsam::GaussianFactor | |
typedef Factor | Base |
Our base class. More... | |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef GaussianFactor | This |
This class. More... | |
Public Types inherited from gtsam::Factor | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
Matrix | augmentedInformation () const override |
Matrix | augmentedJacobian () const override |
Matrix | augmentedJacobianUnweighted () const |
GaussianFactor::shared_ptr | clone () const override |
size_t | cols () const |
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | eliminate (const Ordering &keys) |
bool | equals (const GaussianFactor &lf, double tol=1e-9) const override |
assert equality up to a tolerance More... | |
double | error (const HybridValues &c) const override |
virtual double | error (const VectorValues &c) const |
double | error (const VectorValues &c) const override |
Vector | error_vector (const VectorValues &c) const |
SharedDiagonal & | get_model () |
const SharedDiagonal & | get_model () const |
ABlock | getA () |
constABlock | getA () const |
ABlock | getA (const Key &key) |
constABlock | getA (const_iterator variable) const |
ABlock | getA (iterator variable) |
BVector | getb () |
const constBVector | getb () const |
DenseIndex | getDim (const_iterator variable) const override |
Vector | gradient (Key key, const VectorValues &x) const override |
Compute the gradient wrt a key at any values. More... | |
VectorValues | gradientAtZero () const override |
A'*b for Jacobian. More... | |
void | gradientAtZero (double *d) const override |
A'*b for Jacobian (raw memory version) More... | |
std::map< Key, Matrix > | hessianBlockDiagonal () const override |
Return the block diagonal of the Hessian for this factor. More... | |
void | hessianDiagonal (double *d) const override |
Raw memory access version of hessianDiagonal. More... | |
void | hessianDiagonalAdd (VectorValues &d) const override |
Add the current diagonal to a VectorValues instance. More... | |
Matrix | information () const override |
bool | isConstrained () const |
std::pair< Matrix, Vector > | jacobian () const override |
Returns (dense) A,b pair associated with factor, bakes in the weights. More... | |
JacobianFactor () | |
JacobianFactor (const GaussianFactor &gf) | |
JacobianFactor (const GaussianFactorGraph &graph) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots) | |
JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots) | |
JacobianFactor (const HessianFactor &hf) | |
JacobianFactor (const JacobianFactor &jf) | |
template<typename KEYS > | |
JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) | |
template<typename TERMS > | |
JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
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()) | |
std::pair< Matrix, Vector > | jacobianUnweighted () const |
Returns (dense) A,b pair associated with factor, does not bake in weights. More... | |
VerticalBlockMatrix & | matrixObject () |
const VerticalBlockMatrix & | matrixObject () const |
void | multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const |
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override |
GaussianFactor::shared_ptr | negate () const override |
Vector | operator* (const VectorValues &x) const |
void | print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print with optional string More... | |
size_t | rows () const |
void | setModel (bool anyConstrained, const Vector &sigmas) |
std::shared_ptr< GaussianConditional > | splitConditional (size_t nrFrontals) |
void | transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const |
Vector | unweighted_error (const VectorValues &c) const |
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
JacobianFactor | whiten () const |
~JacobianFactor () override | |
Public Member Functions inherited from gtsam::GaussianFactor | |
GaussianFactor () | |
template<typename CONTAINER > | |
GaussianFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
VectorValues | hessianDiagonal () const |
Return the diagonal of the Hessian for this factor. More... | |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
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. More... | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
Protected Attributes | |
VerticalBlockMatrix | Ab_ |
noiseModel::Diagonal::shared_ptr | model_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Private Member Functions | |
template<class KEYS , class DIMENSIONS > | |
JacobianFactor (const KEYS &keys, const DIMENSIONS &dims, DenseIndex m, const SharedDiagonal &model=SharedDiagonal()) | |
void | JacobianFactorHelper (const GaussianFactorGraph &graph, const FastVector< VariableSlots::const_iterator > &orderedSlots) |
Friends | |
GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys) |
template<typename T > | |
class | ExpressionFactor |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::GaussianFactor | |
template<typename CONTAINER > | |
static DenseIndex | Slot (const CONTAINER &keys, Key key) |
Static Protected Member Functions inherited from gtsam::Factor | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
A Gaussian factor in the squared-error form.
JacobianFactor implements a Gaussian, which has quadratic negative log-likelihood
where is a diagonal covariance matrix. The matrix , r.h.s. vector , and diagonal noise model 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 be a linear measurement prediction function, be the actual observed measurement, the residual is
If we expect noise with diagonal covariance matrix on this measurement, then the negative log-likelihood of the Gaussian induced by this measurement model is
Because is linear, we can write it as
and thus we have
where .
This factor can involve an arbitrary number of variables, and in the above example 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 is passed to these constructors in blocks, for example, for a 2-way factor, the constructor would accept and , as well as the variable indices and and the negative log-likelihood represented by this factor would be
HessianFactor, which represent a Gaussian likelihood over a set of variables.
Definition at line 91 of file JacobianFactor.h.
Definition at line 99 of file JacobianFactor.h.
Typedef to base class.
Definition at line 96 of file JacobianFactor.h.
Definition at line 101 of file JacobianFactor.h.
Definition at line 100 of file JacobianFactor.h.
Definition at line 102 of file JacobianFactor.h.
typedef std::shared_ptr<This> gtsam::JacobianFactor::shared_ptr |
shared_ptr to this class
Definition at line 97 of file JacobianFactor.h.
Typedef to this class.
Definition at line 95 of file JacobianFactor.h.
|
explicit |
Convert from other GaussianFactor
Definition at line 53 of file JacobianFactor.cpp.
|
inline |
Copy constructor
Definition at line 115 of file JacobianFactor.h.
|
explicit |
Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)
Definition at line 90 of file JacobianFactor.cpp.
JacobianFactor::JacobianFactor | ( | ) |
default constructor for I/O
Definition at line 47 of file JacobianFactor.cpp.
|
explicit |
Construct Null factor
Definition at line 65 of file JacobianFactor.cpp.
JacobianFactor::JacobianFactor | ( | Key | i1, |
const Matrix & | A1, | ||
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct unary factor
Definition at line 71 of file JacobianFactor.cpp.
JacobianFactor::JacobianFactor | ( | Key | i1, |
const Matrix & | A1, | ||
Key | i2, | ||
const Matrix & | A2, | ||
const Vector & | b, | ||
const SharedDiagonal & | model = SharedDiagonal() |
||
) |
Construct binary factor
Definition at line 77 of file JacobianFactor.cpp.
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
Definition at line 83 of file JacobianFactor.cpp.
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. |
Definition at line 27 of file JacobianFactor-inl.h.
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.
Definition at line 34 of file JacobianFactor-inl.h.
|
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.
Definition at line 324 of file JacobianFactor.cpp.
|
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.
Definition at line 349 of file JacobianFactor.cpp.
|
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.
Definition at line 374 of file JacobianFactor.cpp.
|
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.
Definition at line 390 of file JacobianFactor.cpp.
|
inlineoverride |
Virtual destructor
Definition at line 187 of file JacobianFactor.h.
|
inlineprivate |
Unsafe Constructor that creates an uninitialized Jacobian of right size
keys | in some order |
diemnsions | of the variables in same order |
m | output dimension |
model | noise model (default nullptr) |
Definition at line 416 of file JacobianFactor.h.
|
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 ). The augmented information matrix is described in more detail in HessianFactor, which in fact stores an augmented information matrix.
Implements gtsam::GaussianFactor.
Definition at line 494 of file JacobianFactor.cpp.
|
overridevirtual |
Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are baked in
Implements gtsam::GaussianFactor.
Definition at line 730 of file JacobianFactor.cpp.
Matrix JacobianFactor::augmentedJacobianUnweighted | ( | ) | const |
Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are not baked in
Definition at line 738 of file JacobianFactor.cpp.
|
inlineoverridevirtual |
Clone this JacobianFactor
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
Definition at line 190 of file JacobianFactor.h.
|
inline |
return the number of columns in the corresponding linear system
Definition at line 289 of file JacobianFactor.h.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > JacobianFactor::eliminate | ( | const Ordering & | keys | ) |
Eliminate the requested variables.
Definition at line 760 of file JacobianFactor.cpp.
|
overridevirtual |
assert equality up to a tolerance
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
Definition at line 420 of file JacobianFactor.cpp.
|
override |
Definition at line 31 of file GaussianFactor.cpp.
double GaussianFactor::error |
Definition at line 27 of file GaussianFactor.cpp.
|
overridevirtual |
Reimplemented from gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
Definition at line 486 of file JacobianFactor.cpp.
Vector JacobianFactor::error_vector | ( | const VectorValues & | c | ) | const |
(A*x-b)
Definition at line 479 of file JacobianFactor.cpp.
|
protected |
Internal function to fill blocks and set dimensions.
Definition at line 60 of file JacobianFactor-inl.h.
|
inline |
get a copy of model (non-const version)
Definition at line 295 of file JacobianFactor.h.
|
inline |
get a copy of model
Definition at line 292 of file JacobianFactor.h.
|
inline |
Get a view of the A matrix
Definition at line 313 of file JacobianFactor.h.
|
inline |
Get a view of the A matrix, not weighted by noise
Definition at line 304 of file JacobianFactor.h.
Get a view of the A matrix for the variable pointed to by the given key.
Definition at line 319 of file JacobianFactor.h.
|
inline |
Get a view of the A matrix for the variable pointed to by the given key iterator
Definition at line 301 of file JacobianFactor.h.
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
Definition at line 310 of file JacobianFactor.h.
|
inline |
Get a view of the r.h.s. vector b (non-const version)
Definition at line 307 of file JacobianFactor.h.
|
inline |
Get a view of the r.h.s. vector b, not weighted by noise
Definition at line 298 of file JacobianFactor.h.
|
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.
Definition at line 277 of file JacobianFactor.h.
|
overridevirtual |
Compute the gradient wrt a key at any values.
Implements gtsam::GaussianFactor.
Definition at line 707 of file JacobianFactor.cpp.
|
overridevirtual |
A'*b for Jacobian.
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularJacobianFactor< D >.
Definition at line 690 of file JacobianFactor.cpp.
|
overridevirtual |
A'*b for Jacobian (raw memory version)
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularJacobianFactor< D >.
Definition at line 702 of file JacobianFactor.cpp.
Return the block diagonal of the Hessian for this factor.
Implements gtsam::GaussianFactor.
Definition at line 550 of file JacobianFactor.cpp.
|
overridevirtual |
Raw memory access version of hessianDiagonal.
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularJacobianFactor< D >.
Definition at line 545 of file JacobianFactor.cpp.
|
overridevirtual |
Add the current diagonal to a VectorValues instance.
Implements gtsam::GaussianFactor.
Definition at line 516 of file JacobianFactor.cpp.
|
overridevirtual |
Return the non-augmented information matrix represented by this GaussianFactor.
Implements gtsam::GaussianFactor.
Definition at line 505 of file JacobianFactor.cpp.
|
inline |
is noise model constrained ?
Definition at line 270 of file JacobianFactor.h.
Returns (dense) A,b pair associated with factor, bakes in the weights.
Implements gtsam::GaussianFactor.
Definition at line 714 of file JacobianFactor.cpp.
|
private |
Helper function for public constructors: Build a dense joint factor from all the factors in a factor graph. Takes in ordered variable slots
Definition at line 206 of file JacobianFactor.cpp.
Returns (dense) A,b pair associated with factor, does not bake in weights.
Definition at line 723 of file JacobianFactor.cpp.
|
inline |
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition at line 260 of file JacobianFactor.h.
|
inline |
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition at line 257 of file JacobianFactor.h.
void 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
Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x Note: this is not assuming a fixed dimension for the variables, but 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 asks: why is this here if not regular ????
Use Eigen magic to access raw memory
Just iterate over all A matrices and multiply in correct config part (looping over keys) E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
Deal with noise properly, need to Double* whiten as we are dividing by variance
multiply with alpha
Again iterate over all A matrices and insert Ai^T into y
Definition at line 653 of file JacobianFactor.cpp.
|
overridevirtual |
y += alpha * A'*A*x
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::RegularJacobianFactor< D >.
Definition at line 638 of file JacobianFactor.cpp.
|
overridevirtual |
Construct the corresponding anti-factor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.
Definition at line 753 of file JacobianFactor.cpp.
Vector JacobianFactor::operator* | ( | const VectorValues & | x | ) | const |
Return A*x
Definition at line 601 of file JacobianFactor.cpp.
|
overridevirtual |
print with optional string
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
Definition at line 403 of file JacobianFactor.cpp.
|
inline |
return the number of rows in the corresponding linear system
Definition at line 284 of file JacobianFactor.h.
void JacobianFactor::setModel | ( | bool | anyConstrained, |
const Vector & | sigmas | ||
) |
set noiseModel correctly
Definition at line 768 of file JacobianFactor.cpp.
GaussianConditional::shared_ptr 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 |
Definition at line 819 of file JacobianFactor.cpp.
void 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.
Definition at line 618 of file JacobianFactor.cpp.
Vector JacobianFactor::unweighted_error | ( | const VectorValues & | c | ) | const |
Definition at line 471 of file JacobianFactor.cpp.
|
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.
Definition at line 563 of file JacobianFactor.cpp.
JacobianFactor JacobianFactor::whiten | ( | ) | const |
Return a whitened version of the factor, i.e. with unit diagonal noise model.
Definition at line 743 of file JacobianFactor.cpp.
|
friend |
Definition at line 422 of file JacobianFactor.h.
|
protected |
Definition at line 106 of file JacobianFactor.h.
|
protected |
Definition at line 107 of file JacobianFactor.h.