#include <LinearEquality.h>
Public Types | |
typedef JacobianFactor | Base |
Typedef to base class. More... | |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef LinearEquality | This |
Typedef to this class. More... | |
Public Types inherited from gtsam::JacobianFactor | |
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 | |
bool | active () const |
for active set method: equality constraints are always active More... | |
GaussianFactor::shared_ptr | clone () const override |
Key | dualKey () const |
dual key More... | |
bool | equals (const GaussianFactor &lf, double tol=1e-9) const override |
double | error (const VectorValues &c) const override |
Vector | error_vector (const VectorValues &c) const |
LinearEquality () | |
LinearEquality (const HessianFactor &hf) | |
LinearEquality (const JacobianFactor &jf, Key dualKey) | |
template<typename TERMS > | |
LinearEquality (const TERMS &terms, const Vector &b, Key dualKey) | |
LinearEquality (Key i1, const Matrix &A1, const Vector &b, Key dualKey) | |
LinearEquality (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, Key dualKey) | |
LinearEquality (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, Key dualKey) | |
void | print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
~LinearEquality () override | |
Public Member Functions inherited from gtsam::JacobianFactor | |
Matrix | augmentedInformation () const override |
Matrix | augmentedJacobian () const override |
Matrix | augmentedJacobianUnweighted () const |
size_t | cols () const |
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | eliminate (const Ordering &keys) |
double | error (const HybridValues &c) const override |
virtual double | error (const VectorValues &c) const |
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 |
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 () |
Private Attributes | |
Key | dualKey_ |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::GaussianFactor | |
template<typename CONTAINER > | |
static DenseIndex | Slot (const CONTAINER &keys, Key key) |
Protected Member Functions inherited from gtsam::JacobianFactor | |
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) | |
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) |
Protected Attributes inherited from gtsam::JacobianFactor | |
VerticalBlockMatrix | Ab_ |
noiseModel::Diagonal::shared_ptr | model_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
This class defines a linear equality constraints, inheriting JacobianFactor with the special Constrained noise model
Definition at line 29 of file LinearEquality.h.
Typedef to base class.
Definition at line 32 of file LinearEquality.h.
typedef std::shared_ptr<This> gtsam::LinearEquality::shared_ptr |
shared_ptr to this class
Definition at line 33 of file LinearEquality.h.
Typedef to this class.
Definition at line 31 of file LinearEquality.h.
|
inline |
default constructor for I/O
Definition at line 40 of file LinearEquality.h.
|
inlineexplicit |
Construct from a constrained noisemodel JacobianFactor with a dual key.
Definition at line 47 of file LinearEquality.h.
|
inlineexplicit |
Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix)
Definition at line 56 of file LinearEquality.h.
|
inline |
Construct unary factor
Definition at line 61 of file LinearEquality.h.
|
inline |
Construct binary factor
Definition at line 66 of file LinearEquality.h.
|
inline |
Construct ternary factor
Definition at line 73 of file LinearEquality.h.
|
inline |
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 83 of file LinearEquality.h.
|
inlineoverride |
Virtual destructor
Definition at line 88 of file LinearEquality.h.
|
inline |
for active set method: equality constraints are always active
Definition at line 114 of file LinearEquality.h.
|
inlineoverridevirtual |
Clone this LinearEquality
Reimplemented from gtsam::JacobianFactor.
Definition at line 103 of file LinearEquality.h.
|
inline |
dual key
Definition at line 109 of file LinearEquality.h.
|
inlineoverridevirtual |
|
inlineoverridevirtual |
Special error for constraints. I think it should be zero, as this function is meant for objective cost. But the name "error" can be misleading. TODO: confirm with Frank!!
Reimplemented from gtsam::JacobianFactor.
Definition at line 127 of file LinearEquality.h.
|
inline |
Special error_vector for constraints (A*x-b)
Definition at line 119 of file LinearEquality.h.
|
inlineoverridevirtual |
|
private |
Definition at line 36 of file LinearEquality.h.