#include <MarginalizationError.hpp>

Inheritance diagram for okvis::ceres::MarginalizationError:

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
Map *	mapPtr_
	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^TJ = _US_U^T , the rhs is decomposed as _b - _HDelta_Chi = -J^T (-pinv(J^T) * _b + JDelta_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 + JDelta_Chi . _e = _e0 + JDelta_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< ParameterBlockInfo >	parameterBlockInfos_
	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] map The 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]	map	The underlying okvis::ceres::Map.
[in]	residualBlockIds	Residual 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]	residualBlockId	Residual block id, the corresponding term of which will be added.
[in]	keepResidualBlock	Currently 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]	residualBlockIds	Vector of residual block ids, the corresponding terms of which will be added.
[in]	keepResidualBlocks	Currently 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

Derived	Matrix type (auto-deducible).
blockDim	The block size of the diagonal blocks.

Parameters

[in]	M_in	Input Matrix
[out]	M_out	Output, i.e. thepseudo-inverse.
[in]	epsilon	The 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

Derived	Matrix type (auto-deducible).
blockDim	The block size of the diagonal blocks.

Parameters

[in]	M_in	Input Matrix
[out]	M_out	Output, i.e. the Cholesky decomposition of a pseudo-inverse.
[in]	epsilon	The 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

parameters	Pointer to the parameters (see ceres)
residuals	Pointer to the residual vector (see ceres)
jacobians	Pointer 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

parameters	Pointer to the parameters (see ceres)
residuals	Pointer to the residual vector (see ceres)
jacobians	Pointer to the Jacobians (see ceres)
jacobiansMinimal	Pointer 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] parameterBlockPtrs Parameter block pointers in question.

bool okvis::ceres::MarginalizationError::isParameterBlockConnected ( uint64_t parameterBlockId )

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

Parameters

[in] parameterBlockId Parameter 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] parameterBlockId ID 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

Derived Matrix type (auto-deducible).

Parameters

[in]	a	Input Matrix
[out]	result	Output, i.e. pseudo-inverse.
[in]	epsilon	The tolerance.
[out]	rank	Optional 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

Derived Matrix type (auto-deducible).

Parameters

[in]	a	Input Matrix
[out]	result	Output, i.e. the Cholesky decomposition of a pseudo-inverse.
[in]	epsilon	The tolerance.
[out]	rank	The 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] map The 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:

okvis_ceres/include/okvis/ceres/MarginalizationError.hpp
okvis_ceres/src/MarginalizationError.cpp

Classes

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation