#include <RegularImplicitSchurFactor.h>
Public Types | |
typedef std::vector< Vector2, Eigen::aligned_allocator< Vector2 > > | Error2s |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef RegularImplicitSchurFactor | 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 |
Compute full augmented information matrix More... | |
Matrix | augmentedJacobian () const override |
const Vector & | b () const |
GaussianFactor::shared_ptr | clone () const override |
const Matrix & | E () const |
bool | equals (const GaussianFactor &lf, double tol) const override |
equals More... | |
double | error (const VectorValues &x) const override |
double | errorJF (const VectorValues &x) const |
const FBlocks & | Fs () const |
DenseIndex | getDim (const_iterator variable) const override |
Degrees of freedom of camera. More... | |
const Matrix & | getPointCovariance () const |
Vector | gradient (Key key, const VectorValues &x) const override |
Gradient wrt a key at any values. More... | |
VectorValues | gradientAtZero () const override |
void | gradientAtZero (double *d) const override |
std::map< Key, Matrix > | hessianBlockDiagonal () const override |
Return the block diagonal of the Hessian for this factor. More... | |
VectorValues | hessianDiagonal () const |
Using the base method. More... | |
void | hessianDiagonal (double *d) const override |
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor More... | |
virtual void | hessianDiagonal (double *d) const=0 |
Using the base method. More... | |
void | hessianDiagonalAdd (VectorValues &d) const override |
Add the diagonal of the Hessian for this factor to existing VectorValues. More... | |
Matrix | information () const override |
Compute full information matrix More... | |
std::pair< Matrix, Vector > | jacobian () const override |
void | multiplyHessianAdd (double alpha, const double *x, double *y) const |
double* Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way More... | |
void | multiplyHessianAdd (double alpha, const double *x, double *y, std::vector< size_t > keys) const |
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override |
Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x. More... | |
void | multiplyHessianDummy (double alpha, const VectorValues &x, VectorValues &y) const |
Dummy version to measure overhead of key access. More... | |
GaussianFactor::shared_ptr | negate () const override |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
print More... | |
void | projectError (const Error2s &e1, Error2s &e2) const |
Calculate corrected error Q*e = (I - E*P*E')*e. More... | |
void | projectError2 (const Error2s &e1, Error2s &e2) const |
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) More... | |
RegularImplicitSchurFactor () | |
Constructor. More... | |
RegularImplicitSchurFactor (const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b) | |
Construct from blocks of F, E, inv(E'*E), and RHS vector b. More... | |
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
~RegularImplicitSchurFactor () override | |
Destructor. More... | |
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 () |
Static Public Member Functions | |
static void | multiplyHessianAdd (const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y) |
Static Public Member Functions inherited from gtsam::GaussianFactor | |
template<typename CONTAINER > | |
static DenseIndex | Slot (const CONTAINER &keys, Key key) |
Public Attributes | |
Error2s | e1 |
Scratch space for multiplyHessianAdd. More... | |
Error2s | e2 |
Protected Types | |
typedef std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > | FBlocks |
typedef Eigen::Matrix< double, D, D > | MatrixDD |
camera Hessian More... | |
typedef Eigen::Matrix< double, ZDim, D > | MatrixZD |
type of an F block More... | |
typedef CameraSet< CAMERA > | Set |
typedef CAMERA::Measurement | Z |
Protected Attributes | |
const Vector | b_ |
2m-dimensional RHS vector More... | |
const Matrix | E_ |
The 2m*3 E Jacobian with respect to the point. More... | |
FBlocks | FBlocks_ |
All ZDim*D F blocks (one for each camera) More... | |
const Matrix | PointCovariance_ |
the 3*3 matrix P = inv(E'E) (2*2 if degenerate) More... | |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Static Protected Attributes | |
static const int | D = traits<CAMERA>::dimension |
Camera dimension. More... | |
static const int | ZDim = traits<Z>::dimension |
Measurement dimension. More... | |
Additional Inherited Members | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
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 specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. Specifically, as measured in timeSchurFactors.cpp, it stays very fast for an increasing number of cameras. The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at the core of CG, and implements y += F'alpha(I - E*P*E')*F*x where
Definition at line 39 of file RegularImplicitSchurFactor.h.
typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2> > gtsam::RegularImplicitSchurFactor< CAMERA >::Error2s |
Definition at line 282 of file RegularImplicitSchurFactor.h.
|
protected |
Definition at line 56 of file RegularImplicitSchurFactor.h.
|
protected |
camera Hessian
Definition at line 55 of file RegularImplicitSchurFactor.h.
|
protected |
type of an F block
Definition at line 54 of file RegularImplicitSchurFactor.h.
|
protected |
Definition at line 48 of file RegularImplicitSchurFactor.h.
typedef std::shared_ptr<This> gtsam::RegularImplicitSchurFactor< CAMERA >::shared_ptr |
shared_ptr to this class
Definition at line 43 of file RegularImplicitSchurFactor.h.
typedef RegularImplicitSchurFactor gtsam::RegularImplicitSchurFactor< CAMERA >::This |
Typedef to this class.
Definition at line 42 of file RegularImplicitSchurFactor.h.
|
protected |
Definition at line 50 of file RegularImplicitSchurFactor.h.
|
inline |
Constructor.
Definition at line 66 of file RegularImplicitSchurFactor.h.
|
inline |
Construct from blocks of F, E, inv(E'*E), and RHS vector b.
Construct a new RegularImplicitSchurFactor object.
keys | keys corresponding to cameras |
Fs | All ZDim*D F blocks (one for each camera) |
E | Jacobian of measurements wrpt point. |
P | point covariance matrix |
b | RHS vector |
Definition at line 80 of file RegularImplicitSchurFactor.h.
|
inlineoverride |
Destructor.
Definition at line 85 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Compute full augmented information matrix
Implements gtsam::GaussianFactor.
Definition at line 155 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b. The negative log-likelihood is . See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
Implements gtsam::GaussianFactor.
Definition at line 143 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 96 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Clone a factor (make a deep copy)
Implements gtsam::GaussianFactor.
Definition at line 256 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 92 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
equals
Implements gtsam::GaussianFactor.
Definition at line 118 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Reimplemented from gtsam::GaussianFactor.
Definition at line 314 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 336 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 88 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Degrees of freedom of camera.
Implements gtsam::GaussianFactor.
Definition at line 134 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 100 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Gradient wrt a key at any values.
Implements gtsam::GaussianFactor.
Definition at line 499 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Calculate gradient, which is -F'Q*b, see paper
Implements gtsam::GaussianFactor.
Definition at line 457 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
Implements gtsam::GaussianFactor.
Definition at line 479 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Return the block diagonal of the Hessian for this factor.
Implements gtsam::GaussianFactor.
Definition at line 231 of file RegularImplicitSchurFactor.h.
VectorValues GaussianFactor::hessianDiagonal |
Using the base method.
Definition at line 35 of file GaussianFactor.cpp.
|
inlineoverridevirtual |
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor
Implements gtsam::GaussianFactor.
Definition at line 205 of file RegularImplicitSchurFactor.h.
virtual void gtsam::GaussianFactor::hessianDiagonal |
Using the base method.
|
inlineoverridevirtual |
Add the diagonal of the Hessian for this factor to existing VectorValues.
Implements gtsam::GaussianFactor.
Definition at line 174 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Compute full information matrix
Implements gtsam::GaussianFactor.
Definition at line 163 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b. The negative log-likelihood is . See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.
Implements gtsam::GaussianFactor.
Definition at line 148 of file RegularImplicitSchurFactor.h.
|
inlinestatic |
Definition at line 272 of file RegularImplicitSchurFactor.h.
|
inline |
double* Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
Definition at line 380 of file RegularImplicitSchurFactor.h.
|
inline |
Definition at line 406 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x.
Implements gtsam::GaussianFactor.
Definition at line 413 of file RegularImplicitSchurFactor.h.
|
inline |
Dummy version to measure overhead of key access.
Definition at line 442 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Construct the corresponding anti-factor to negate information stored stored in this factor.
Implements gtsam::GaussianFactor.
Definition at line 263 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
Implements gtsam::GaussianFactor.
Definition at line 105 of file RegularImplicitSchurFactor.h.
|
inline |
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition at line 357 of file RegularImplicitSchurFactor.h.
|
inline |
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
Definition at line 287 of file RegularImplicitSchurFactor.h.
|
inlineoverridevirtual |
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 138 of file RegularImplicitSchurFactor.h.
|
protected |
2m-dimensional RHS vector
Definition at line 61 of file RegularImplicitSchurFactor.h.
|
staticprotected |
Camera dimension.
Definition at line 51 of file RegularImplicitSchurFactor.h.
|
mutable |
Scratch space for multiplyHessianAdd.
Definition at line 374 of file RegularImplicitSchurFactor.h.
Error2s gtsam::RegularImplicitSchurFactor< CAMERA >::e2 |
Definition at line 374 of file RegularImplicitSchurFactor.h.
|
protected |
The 2m*3 E Jacobian with respect to the point.
Definition at line 60 of file RegularImplicitSchurFactor.h.
|
protected |
All ZDim*D F blocks (one for each camera)
Definition at line 58 of file RegularImplicitSchurFactor.h.
|
protected |
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition at line 59 of file RegularImplicitSchurFactor.h.
|
staticprotected |
Measurement dimension.
Definition at line 52 of file RegularImplicitSchurFactor.h.