OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Classes | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
okvis::ceres::MarginalizationError Class Reference

#include <MarginalizationError.hpp>

Inheritance diagram for okvis::ceres::MarginalizationError:
okvis::ceres::ErrorInterface

Classes

struct  ParameterBlockInfo
 Book-keeping of the ordering. More...
 

Public Member Functions

virtual ~MarginalizationError ()
 Trivial destructor. More...
 
 MarginalizationError ()
 Default constructor. Initialises a new okvis::ceres::Map. More...
 
 MarginalizationError (Map &map)
 Constructor from okvis::ceres::Map. More...
 
 MarginalizationError (Map &map, std::vector< ::ceres::ResidualBlockId > &residualBlockIds)
 Constructor from okvis::ceres::Map and directly add some residuals. More...
 
void setMap (Map &map)
 Set the underlying okvis::ceres::Map. More...
 
bool addResidualBlocks (const std::vector< ::ceres::ResidualBlockId > &residualBlockIds, const std::vector< bool > &keepResidualBlocks=std::vector< bool >())
 Add some residuals to this marginalisation error. This means, they will get linearised. More...
 
bool addResidualBlock (::ceres::ResidualBlockId residualBlockId, bool keepResidualBlock=false)
 Add one residual to this marginalisation error. This means, it will get linearised. More...
 
bool isParameterBlockConnected (uint64_t parameterBlockId)
 Info: is this parameter block connected to this marginalization error? More...
 
bool marginalizeOut (const std::vector< uint64_t > &parameterBlockIds, const std::vector< bool > &keepParameterBlocks=std::vector< bool >())
 Marginalise out a set of parameter blocks. More...
 
void updateErrorComputation ()
 This must be called before optimization after adding residual blocks and/or marginalizing, since it performs all the lhs and rhs computations on from a given _H and _b. More...
 
void getParameterBlockPtrs (std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
 Call this in order to (re-)add this error term after whenever it had been modified. More...
 
virtual bool Evaluate (double const *const *parameters, double *residuals, double **jacobians) const
 This evaluates the error term and additionally computes the Jacobians. More...
 
bool EvaluateWithMinimalJacobians (double const *const *parameters, double *residuals, double **jacobians, double **jacobiansMinimal) const
 This evaluates the error term and additionally computes the Jacobians in the minimal internal representation. More...
 
size_t residualDim () const
 Residual dimension. More...
 
size_t parameterBlocks () const
 Number of parameter blocks. More...
 
size_t parameterBlockDim (size_t parameterBlockId) const
 Dimension of an individual parameter block. More...
 
virtual std::string typeInfo () const
 Return parameter block type as string. More...
 
- Public Member Functions inherited from okvis::ceres::ErrorInterface
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ErrorInterface ()
 Constructor. More...
 
virtual ~ErrorInterface ()
 Destructor (does nothing). More...
 

Static Public Member Functions

template<typename Derived >
static bool pseudoInverseSymm (const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &result, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon(), int *rank=0)
 Pseudo inversion of a symmetric matrix. More...
 
template<typename Derived >
static bool pseudoInverseSymmSqrt (const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &result, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon(), int *rank=NULL)
 Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix. More...
 
template<typename Derived , int blockDim>
static void blockPinverse (const Eigen::MatrixBase< Derived > &M_in, const Eigen::MatrixBase< Derived > &M_out, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon())
 Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks. More...
 
template<typename Derived , int blockDim>
static void blockPinverseSqrt (const Eigen::MatrixBase< Derived > &M_in, const Eigen::MatrixBase< Derived > &M_out, double epsilon=std::numeric_limits< typename Derived::Scalar >::epsilon())
 Block-wise pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix with non-zero diagonal blocks. More...
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef::ceres::CostFunction 
base_t
 The base class type. More...
 

Protected Member Functions

void check ()
 Checks the internal datastructure (debug) More...
 
bool computeDeltaChi (Eigen::VectorXd &DeltaChi) const
 Computes the linearized deviation from the references (linearization points) More...
 
bool computeDeltaChi (double const *const *parameters, Eigen::VectorXd &DeltaChi) const
 Computes the linearized deviation from the references (linearization points) More...
 

Static Protected Member Functions

template<typename Derived_A , typename Derived_U , typename Derived_W , typename Derived_V >
static void splitSymmetricMatrix (const std::vector< std::pair< int, int > > &marginalizationStartIdxAndLengthPairs, const Eigen::MatrixBase< Derived_A > &A, const Eigen::MatrixBase< Derived_U > &U, const Eigen::MatrixBase< Derived_W > &W, const Eigen::MatrixBase< Derived_V > &V)
 Split for Schur complement op. More...
 
template<typename Derived_b , typename Derived_b_a , typename Derived_b_b >
static void splitVector (const std::vector< std::pair< int, int > > &marginalizationStartIdxAndLengthPairs, const Eigen::MatrixBase< Derived_b > &b, const Eigen::MatrixBase< Derived_b_a > &b_a, const Eigen::MatrixBase< Derived_b_b > &b_b)
 Split for Schur complement op. More...
 

Protected Attributes

MapmapPtr_
 The underlying map. More...
 
::ceres::ResidualBlockId residualBlockId_
 The residual block id of this. More...
 
The internal storage of the linearised system.

lhs and rhs: H_*delta_Chi = b - H*Delta_Chi . the lhs Hessian matrix is decomposed as _H = J^T*J = _U*S*_U^T , the rhs is decomposed as _b - _H*Delta_Chi = -J^T * (-pinv(J^T) * _b + J*Delta_Chi) , i.e. we have the ceres standard form with weighted Jacobians _J, an identity information matrix, and an error _e = -pinv(J^T) * _b + J*Delta_Chi . _e = _e0 + J*Delta_Chi .

Eigen::MatrixXd H_
 lhs - Hessian More...
 
Eigen::VectorXd b0_
 rhs constant part More...
 
Eigen::VectorXd e0_
 _e0 := pinv(J^T) * _b0 More...
 
Eigen::MatrixXd J_
 Jacobian such that _J^T * J == _H. More...
 
Eigen::MatrixXd U_
 H_ = _U*_S*_U^T lhs Eigen decomposition. More...
 
Eigen::VectorXd S_
 singular values More...
 
Eigen::VectorXd S_sqrt_
 cwise sqrt of _S, i.e. _S_sqrt*_S_sqrt=_S; _J=_U^T*_S_sqrt More...
 
Eigen::VectorXd S_pinv_
 pseudo inverse of _S More...
 
Eigen::VectorXd S_pinv_sqrt_
 cwise sqrt of _S_pinv, i.e. pinv(J^T)=_U^T*_S_pinv_sqrt More...
 
Eigen::VectorXd p_
 
Eigen::VectorXd p_inv_
 
volatile bool errorComputationValid_
 adding residual blocks will invalidate this. before optimizing, call updateErrorComputation() More...
 
std::vector< ParameterBlockInfoparameterBlockInfos_
 Book keeper. More...
 
std::map< uint64_t, size_t > parameterBlockId2parameterBlockInfoIdx_
 Maps parameter block Ids to index in _parameterBlockInfos. More...
 
size_t denseIndices_
 Keep track of the size of the dense part of the equation system. More...
 

Constructor & Destructor Documentation

virtual okvis::ceres::MarginalizationError::~MarginalizationError ( )
inlinevirtual

Trivial destructor.

okvis::ceres::MarginalizationError::MarginalizationError ( )

Default constructor. Initialises a new okvis::ceres::Map.

okvis::ceres::MarginalizationError::MarginalizationError ( Map map)

Constructor from okvis::ceres::Map.

Parameters
[in]mapThe underlying okvis::ceres::Map.
okvis::ceres::MarginalizationError::MarginalizationError ( Map map,
std::vector< ::ceres::ResidualBlockId > &  residualBlockIds 
)

Constructor from okvis::ceres::Map and directly add some residuals.

Parameters
[in]mapThe underlying okvis::ceres::Map.
[in]residualBlockIdsResidual block IDs to be added directly (
See Also
okvis::ceres::addResidualBlocks)

Member Function Documentation

bool okvis::ceres::MarginalizationError::addResidualBlock ( ::ceres::ResidualBlockId  residualBlockId,
bool  keepResidualBlock = false 
)

Add one residual to this marginalisation error. This means, it will get linearised.

Warning
Note that once added here, it will be removed from the okvis::ceres::Map and stay linerised at exactly the point passed here.
Parameters
[in]residualBlockIdResidual block id, the corresponding term of which will be added.
[in]keepResidualBlockCurrently not in use.
bool okvis::ceres::MarginalizationError::addResidualBlocks ( const std::vector< ::ceres::ResidualBlockId > &  residualBlockIds,
const std::vector< bool > &  keepResidualBlocks = std::vector<bool>() 
)

Add some residuals to this marginalisation error. This means, they will get linearised.

Warning
Note that once added here, they will be removed from the okvis::ceres::Map and stay linerised at exactly the points passed here.
Parameters
[in]residualBlockIdsVector of residual block ids, the corresponding terms of which will be added.
[in]keepResidualBlocksCurrently not in use.
template<typename Derived , int blockDim>
void okvis::ceres::MarginalizationError::blockPinverse ( const Eigen::MatrixBase< Derived > &  M_in,
const Eigen::MatrixBase< Derived > &  M_out,
double  epsilon = std::numeric_limits<typename Derived::Scalar>::epsilon() 
)
static

Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks.

Warning
This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite (negative Eigenvalues are set to zero).
Template Parameters
DerivedMatrix type (auto-deducible).
blockDimThe block size of the diagonal blocks.
Parameters
[in]M_inInput Matrix
[out]M_outOutput, i.e. thepseudo-inverse.
[in]epsilonThe tolerance.
Returns
template<typename Derived , int blockDim>
void okvis::ceres::MarginalizationError::blockPinverseSqrt ( const Eigen::MatrixBase< Derived > &  M_in,
const Eigen::MatrixBase< Derived > &  M_out,
double  epsilon = std::numeric_limits<typename Derived::Scalar>::epsilon() 
)
static

Block-wise pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix with non-zero diagonal blocks.

Warning
This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite (negative Eigenvalues are set to zero).
Template Parameters
DerivedMatrix type (auto-deducible).
blockDimThe block size of the diagonal blocks.
Parameters
[in]M_inInput Matrix
[out]M_outOutput, i.e. the Cholesky decomposition of a pseudo-inverse.
[in]epsilonThe tolerance.
Returns
void okvis::ceres::MarginalizationError::check ( )
protected

Checks the internal datastructure (debug)

bool okvis::ceres::MarginalizationError::computeDeltaChi ( Eigen::VectorXd &  DeltaChi) const
protected

Computes the linearized deviation from the references (linearization points)

bool okvis::ceres::MarginalizationError::computeDeltaChi ( double const *const *  parameters,
Eigen::VectorXd &  DeltaChi 
) const
protected

Computes the linearized deviation from the references (linearization points)

bool okvis::ceres::MarginalizationError::Evaluate ( double const *const *  parameters,
double *  residuals,
double **  jacobians 
) const
virtual

This evaluates the error term and additionally computes the Jacobians.

Parameters
parametersPointer to the parameters (see ceres)
residualsPointer to the residual vector (see ceres)
jacobiansPointer to the Jacobians (see ceres)
Returns
success of th evaluation.
bool okvis::ceres::MarginalizationError::EvaluateWithMinimalJacobians ( double const *const *  parameters,
double *  residuals,
double **  jacobians,
double **  jacobiansMinimal 
) const
virtual

This evaluates the error term and additionally computes the Jacobians in the minimal internal representation.

Parameters
parametersPointer to the parameters (see ceres)
residualsPointer to the residual vector (see ceres)
jacobiansPointer to the Jacobians (see ceres)
jacobiansMinimalPointer to the minimal Jacobians (equivalent to jacobians).
Returns
Success of the evaluation.

Implements okvis::ceres::ErrorInterface.

void okvis::ceres::MarginalizationError::getParameterBlockPtrs ( std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &  parameterBlockPtrs)

Call this in order to (re-)add this error term after whenever it had been modified.

Parameters
[in]parameterBlockPtrsParameter block pointers in question.
bool okvis::ceres::MarginalizationError::isParameterBlockConnected ( uint64_t  parameterBlockId)

Info: is this parameter block connected to this marginalization error?

Parameters
[in]parameterBlockIdParameter block id of interest.
bool okvis::ceres::MarginalizationError::marginalizeOut ( const std::vector< uint64_t > &  parameterBlockIds,
const std::vector< bool > &  keepParameterBlocks = std::vector<bool>() 
)

Marginalise out a set of parameter blocks.

Warning
The parameter blocks to be marginalised must have been added before. Also: make sure to add all the needed residual blocks beforehand for this to make sense.
Returns
False if not all necessary residual blocks were added before.
size_t okvis::ceres::MarginalizationError::parameterBlockDim ( size_t  parameterBlockId) const
inlinevirtual

Dimension of an individual parameter block.

Parameters
[in]parameterBlockIdID of the parameter block of interest.
Returns
The dimension.

Implements okvis::ceres::ErrorInterface.

size_t okvis::ceres::MarginalizationError::parameterBlocks ( ) const
inlinevirtual

Number of parameter blocks.

Implements okvis::ceres::ErrorInterface.

template<typename Derived >
bool okvis::ceres::MarginalizationError::pseudoInverseSymm ( const Eigen::MatrixBase< Derived > &  a,
const Eigen::MatrixBase< Derived > &  result,
double  epsilon = std::numeric_limits<typename Derived::Scalar>::epsilon(),
int *  rank = 0 
)
static

Pseudo inversion of a symmetric matrix.

Warning
This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite (negative Eigenvalues are set to zero).
Template Parameters
DerivedMatrix type (auto-deducible).
Parameters
[in]aInput Matrix
[out]resultOutput, i.e. pseudo-inverse.
[in]epsilonThe tolerance.
[out]rankOptional rank.
Returns
template<typename Derived >
bool okvis::ceres::MarginalizationError::pseudoInverseSymmSqrt ( const Eigen::MatrixBase< Derived > &  a,
const Eigen::MatrixBase< Derived > &  result,
double  epsilon = std::numeric_limits<typename Derived::Scalar>::epsilon(),
int *  rank = NULL 
)
static

Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix.

Warning
This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite (negative Eigenvalues are set to zero).
Template Parameters
DerivedMatrix type (auto-deducible).
Parameters
[in]aInput Matrix
[out]resultOutput, i.e. the Cholesky decomposition of a pseudo-inverse.
[in]epsilonThe tolerance.
[out]rankThe rank, if of interest.
Returns
size_t okvis::ceres::MarginalizationError::residualDim ( ) const
inlinevirtual

Residual dimension.

Implements okvis::ceres::ErrorInterface.

void okvis::ceres::MarginalizationError::setMap ( Map map)

Set the underlying okvis::ceres::Map.

Parameters
[in]mapThe underlying okvis::ceres::Map.
template<typename Derived_A , typename Derived_U , typename Derived_W , typename Derived_V >
void okvis::ceres::MarginalizationError::splitSymmetricMatrix ( const std::vector< std::pair< int, int > > &  marginalizationStartIdxAndLengthPairs,
const Eigen::MatrixBase< Derived_A > &  A,
const Eigen::MatrixBase< Derived_U > &  U,
const Eigen::MatrixBase< Derived_W > &  W,
const Eigen::MatrixBase< Derived_V > &  V 
)
staticprotected

Split for Schur complement op.

template<typename Derived_b , typename Derived_b_a , typename Derived_b_b >
void okvis::ceres::MarginalizationError::splitVector ( const std::vector< std::pair< int, int > > &  marginalizationStartIdxAndLengthPairs,
const Eigen::MatrixBase< Derived_b > &  b,
const Eigen::MatrixBase< Derived_b_a > &  b_a,
const Eigen::MatrixBase< Derived_b_b > &  b_b 
)
staticprotected

Split for Schur complement op.

virtual std::string okvis::ceres::MarginalizationError::typeInfo ( ) const
inlinevirtual

Return parameter block type as string.

Implements okvis::ceres::ErrorInterface.

void okvis::ceres::MarginalizationError::updateErrorComputation ( )

This must be called before optimization after adding residual blocks and/or marginalizing, since it performs all the lhs and rhs computations on from a given _H and _b.

Member Data Documentation

Eigen::VectorXd okvis::ceres::MarginalizationError::b0_
protected

rhs constant part

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef ::ceres::CostFunction okvis::ceres::MarginalizationError::base_t

The base class type.

size_t okvis::ceres::MarginalizationError::denseIndices_
protected

Keep track of the size of the dense part of the equation system.

Eigen::VectorXd okvis::ceres::MarginalizationError::e0_
protected

_e0 := pinv(J^T) * _b0

volatile bool okvis::ceres::MarginalizationError::errorComputationValid_
protected

adding residual blocks will invalidate this. before optimizing, call updateErrorComputation()

Eigen::MatrixXd okvis::ceres::MarginalizationError::H_
protected

lhs - Hessian

Eigen::MatrixXd okvis::ceres::MarginalizationError::J_
protected

Jacobian such that _J^T * J == _H.

Map* okvis::ceres::MarginalizationError::mapPtr_
protected

The underlying map.

Eigen::VectorXd okvis::ceres::MarginalizationError::p_
protected
Eigen::VectorXd okvis::ceres::MarginalizationError::p_inv_
protected
std::map<uint64_t, size_t> okvis::ceres::MarginalizationError::parameterBlockId2parameterBlockInfoIdx_
protected

Maps parameter block Ids to index in _parameterBlockInfos.

std::vector<ParameterBlockInfo> okvis::ceres::MarginalizationError::parameterBlockInfos_
protected

Book keeper.

::ceres::ResidualBlockId okvis::ceres::MarginalizationError::residualBlockId_
protected

The residual block id of this.

Eigen::VectorXd okvis::ceres::MarginalizationError::S_
protected

singular values

Eigen::VectorXd okvis::ceres::MarginalizationError::S_pinv_
protected

pseudo inverse of _S

Eigen::VectorXd okvis::ceres::MarginalizationError::S_pinv_sqrt_
protected

cwise sqrt of _S_pinv, i.e. pinv(J^T)=_U^T*_S_pinv_sqrt

Eigen::VectorXd okvis::ceres::MarginalizationError::S_sqrt_
protected

cwise sqrt of _S, i.e. _S_sqrt*_S_sqrt=_S; _J=_U^T*_S_sqrt

Eigen::MatrixXd okvis::ceres::MarginalizationError::U_
protected

H_ = _U*_S*_U^T lhs Eigen decomposition.


The documentation for this class was generated from the following files: