template<class CAMERA>
class gtsam::RegularImplicitSchurFactor< CAMERA >
RegularImplicitSchurFactor.
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
- F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
- E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
- P is the covariance on the point The equation above implicitly executes the Schur complement by removing the information E*P*E' from the Hessian. It is also very fast as we do not use the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.
|
| RegularImplicitSchurFactor () |
| Constructor.
|
|
| 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.
|
|
| ~RegularImplicitSchurFactor () override |
| Destructor.
|
|
const FBlocks & | Fs () const |
|
const Matrix & | E () const |
|
const Vector & | b () const |
|
const Matrix & | getPointCovariance () const |
|
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| print
|
|
bool | equals (const GaussianFactor &lf, double tol) const override |
| equals
|
|
DenseIndex | getDim (const_iterator variable) const override |
| Degrees of freedom of camera.
|
|
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
| Update an information matrix by adding the information corresponding to this factor (used internally during elimination).
|
|
Matrix | augmentedJacobian () const override |
| Return a dense \( [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \) Jacobian matrix, augmented with b with the noise models baked into A and b.
|
|
std::pair< Matrix, Vector > | jacobian () const override |
| Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b.
|
|
Matrix | augmentedInformation () const override |
| Compute full augmented information matrix
|
|
Matrix | information () const override |
| Compute full information matrix
|
|
void | hessianDiagonalAdd (VectorValues &d) const override |
| Add the diagonal of the Hessian for this factor to existing VectorValues.
|
|
void | hessianDiagonal (double *d) const override |
| add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessianFactor
|
|
std::map< Key, Matrix > | hessianBlockDiagonal () const override |
| Return the block diagonal of the Hessian for this factor.
|
|
GaussianFactor::shared_ptr | clone () const override |
| Clone a factor (make a deep copy)
|
|
GaussianFactor::shared_ptr | negate () const override |
| Construct the corresponding anti-factor to negate information stored stored in this factor.
|
|
void | projectError2 (const Error2s &e1, Error2s &e2) const |
| Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
|
|
double | error (const VectorValues &x) const override |
|
double | errorJF (const VectorValues &x) const |
|
void | projectError (const Error2s &e1, Error2s &e2) const |
| Calculate corrected error Q*e = (I - E*P*E')*e.
|
|
void | multiplyHessianAdd (double alpha, const double *x, double *y) const |
| double* Hessian-vector multiply, i.e.
|
|
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.
|
|
void | multiplyHessianDummy (double alpha, const VectorValues &x, VectorValues &y) const |
| Dummy version to measure overhead of key access.
|
|
VectorValues | gradientAtZero () const override |
| Calculate gradient, which is -F'Q*b, see paper.
|
|
void | gradientAtZero (double *d) const override |
| Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
|
|
Vector | gradient (Key key, const VectorValues &x) const override |
| Gradient wrt a key at any values.
|
|
VectorValues | hessianDiagonal () const |
| Using the base method.
|
|
virtual void | hessianDiagonal (double *d) const=0 |
| Using the base method.
|
|
| GaussianFactor () |
| Default constructor creates empty factor.
|
|
template<typename CONTAINER > |
| GaussianFactor (const CONTAINER &keys) |
| Construct from container of keys.
|
|
virtual | ~GaussianFactor () |
| Destructor.
|
|
double | error (const HybridValues &c) const override |
| All factor types need to implement an error function.
|
|
VectorValues | hessianDiagonal () const |
| Return the diagonal of the Hessian for this factor.
|
|
virtual | ~Factor ()=default |
| Default destructor.
|
|
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 |
| Iterator at beginning of involved variable keys.
|
|
const_iterator | end () const |
| Iterator at end of involved variable keys.
|
|
size_t | size () const |
|
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
| print only keys
|
|
bool | equals (const This &other, double tol=1e-9) const |
| check equality
|
|
KeyVector & | keys () |
|
iterator | begin () |
| Iterator at beginning of involved variable keys.
|
|
iterator | end () |
| Iterator at end of involved variable keys.
|
|