GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#include <JacobianFactorSVD.h>
Public Types | |
typedef JacobianFactor | This |
Typedef to this 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 | |
JacobianFactorSVD () | |
Default constructor. | |
JacobianFactorSVD (const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal()) | |
Empty constructor with keys. | |
JacobianFactorSVD (const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet. More... | |
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override |
void | multiplyHessianAdd (double alpha, const double *x, double *y) const |
double* Hessian-vector multiply, i.e. y += A'*(A*x) RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way | |
void | multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const |
void | hessianDiagonal (double *d) const override |
Raw memory access version of hessianDiagonal. | |
VectorValues | gradientAtZero () const override |
Expose base class gradientAtZero. | |
void | gradientAtZero (double *d) const override |
Raw memory access version of gradientAtZero. | |
void | transposeMultiplyAdd (double alpha, const Vector &e, double *x) const |
double* Transpose Matrix-vector multiply, i.e. x += A'*e RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way | |
void | transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const |
Vector | operator* (const double *x) const |
double* Matrix-vector multiply, i.e. y = A*x RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way | |
Vector | operator* (const VectorValues &x) const |
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. | |
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 |
BVector | getb () |
constABlock | getA (const_iterator variable) const |
constABlock | getA () const |
ABlock | getA (iterator variable) |
ABlock | getA () |
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
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) |
Standard Interface | |
VectorValues | hessianDiagonal () const |
Return the diagonal of the Hessian for this factor. | |
double | error (const HybridValues &c) const override |
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 | |
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. | |
JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.
This trick is equivalent to the Schur complement, but can be faster. In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 multiplication.
|
inline |
Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet.
keys | keys associated with F blocks. |
Fblocks | CameraSet derivatives, ZDim*D blocks of block-diagonal F |
Enull | a reduced point derivative |
b | right-hand side |
model | noise model |
|
overridevirtualinherited |
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.
|
overridevirtualinherited |
Return (dense) matrix associated with factor. The returned system is an augmented matrix: [A b] weights are baked in
Implements gtsam::GaussianFactor.
|
inherited |
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
|
inlineoverridevirtualinherited |
Clone this JacobianFactor
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::LinearInequality, gtsam::LinearEquality, and gtsam::LinearCost.
|
inlineinherited |
return the number of columns in the corresponding linear system
|
inherited |
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.
|
inherited |
(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.
|
inlineinherited |
get a copy of model
|
inlineinherited |
get a copy of model (non-const version)
|
inlineinherited |
Get a view of the A matrix for the variable pointed to by the given key iterator
|
inlineinherited |
Get a view of the A matrix, not weighted by noise
|
inlineinherited |
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
|
inlineinherited |
Get a view of the A matrix
|
inlineinherited |
Get a view of the r.h.s. vector b, not weighted by noise
|
inlineinherited |
Get a view of the r.h.s. vector b (non-const version)
|
inlineoverridevirtualinherited |
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.
|
overridevirtualinherited |
Return the non-augmented information matrix represented by this GaussianFactor.
Implements gtsam::GaussianFactor.
|
inlineinherited |
is noise model constrained ?
|
inlineinherited |
|
inlineinherited |
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
|
inlineinherited |
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
|
inlineoverridevirtualinherited |
y += alpha * A'*A*x
Reimplemented from gtsam::JacobianFactor.
|
inherited |
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
|
overridevirtualinherited |
Construct the corresponding anti-factor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.
|
inherited |
Return A*x
|
inlineinherited |
return the number of rows in the corresponding linear system
|
inherited |
set noiseModel correctly
|
inlineinherited |
|
inherited |
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 |
|
inherited |
x += alpha * A'*e. If x is initially missing any values, they are created and assumed to start as zero vectors.
|
overridevirtualinherited |
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.
|
inherited |
Return a whitened version of the factor, i.e. with unit diagonal noise model.