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

The Map class. This keeps track of how parameter blocks are connected to residual blocks. In essence, it encapsulates the ceres::Problem. This way, we can easily manipulate the optimisation problem. You could argue why not use cere's internal mechanisms to do that. We found that our implementation was faster... More...

#include <Map.hpp>

Classes

struct  ResidualBlockSpec
 Struct to store some infos about a residual. More...
 

Public Types

enum  Parameterization {
  HomogeneousPoint, Pose6d, Pose3d, Pose4d,
  Pose2d, Trivial
}
 The Parameterisation enum. More...
 
typedef std::pair< uint64_t,
std::shared_ptr
< okvis::ceres::ParameterBlock > > 
ParameterBlockSpec
 
typedef std::vector
< ResidualBlockSpec
ResidualBlockCollection
 
typedef std::vector
< ParameterBlockSpec
ParameterBlockCollection
 
typedef std::unordered_map
< uint64_t, std::shared_ptr
< okvis::ceres::ParameterBlock > > 
Id2ParameterBlock_Map
 The actual map from Id to parameter block pointer. More...
 
typedef std::unordered_map
< ::ceres::ResidualBlockId,
ResidualBlockSpec
ResidualBlockId2ResidualBlockSpec_Map
 The actual map from Id to residual block specs. More...
 

Public Member Functions

 Map ()
 Constructor. More...
 
bool parameterBlockExists (uint64_t parameterBlockId) const
 Check whether a certain parameter block is part of the map. More...
 
void getLhs (uint64_t parameterBlockId, Eigen::MatrixXd &H)
 Obtain the Hessian block for a specific parameter block. More...
 
bool isJacobianCorrect (::ceres::ResidualBlockId residualBlockId, double relTol=1e-6) const
 Check a Jacobian with numeric differences. More...
 
const Id2ParameterBlock_Mapid2parameterBlockMap () const
 Get map connecting parameter block IDs to parameter blocks. More...
 
const
ResidualBlockId2ResidualBlockSpec_Map
residualBlockId2ResidualBlockSpecMap () const
 Get the actual map from Id to residual block specs. More...
 
void solve ()
 Solve the optimization problem. More...
 
Print info
void printParameterBlockInfo (uint64_t parameterBlockId) const
 Log information on a parameter block. More...
 
void printResidualBlockInfo (::ceres::ResidualBlockId residualBlockId) const
 Log information on a residual block. More...
 
add/remove
bool addParameterBlock (std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock, int parameterization=Parameterization::Trivial, const int group=-1)
 Add a parameter block to the map. More...
 
bool removeParameterBlock (uint64_t parameterBlockId)
 Remove a parameter block from the map. More...
 
bool removeParameterBlock (std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock)
 Remove a parameter block from the map. More...
 
::ceres::ResidualBlockId addResidualBlock (std::shared_ptr< ::ceres::CostFunction > cost_function,::ceres::LossFunction *loss_function, std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
 Adds a residual block. More...
 
void resetResidualBlock (::ceres::ResidualBlockId residualBlockId, std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
 Replace the parameters connected to a residual block ID. More...
 
::ceres::ResidualBlockId addResidualBlock (std::shared_ptr< ::ceres::CostFunction > cost_function,::ceres::LossFunction *loss_function, std::shared_ptr< okvis::ceres::ParameterBlock > x0, std::shared_ptr< okvis::ceres::ParameterBlock > x1=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x2=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x3=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x4=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x5=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x6=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x7=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x8=std::shared_ptr< okvis::ceres::ParameterBlock >(), std::shared_ptr< okvis::ceres::ParameterBlock > x9=std::shared_ptr< okvis::ceres::ParameterBlock >())
 Add a residual block. See respective ceres docu. If more are needed, see other interface. More...
 
bool removeResidualBlock (::ceres::ResidualBlockId id)
 Remove a residual block. More...
 
Set constant/variable/local parameterization
bool setParameterBlockConstant (uint64_t parameterBlockId)
 Do not optimise a certain parameter block. More...
 
bool setParameterBlockVariable (uint64_t parameterBlockId)
 Optimise a certain parameter block (this is the default). More...
 
bool setParameterBlockConstant (std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock)
 Do not optimise a certain parameter block. More...
 
bool setParameterBlockVariable (std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock)
 Optimise a certain parameter block (this is the default). More...
 
bool resetParameterization (uint64_t parameterBlockId, int parameterization)
 Reset the (local) parameterisation of a parameter block. More...
 
bool setParameterization (uint64_t parameterBlockId,::ceres::LocalParameterization *local_parameterization)
 Set the (local) parameterisation of a parameter block. More...
 
bool setParameterization (std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock,::ceres::LocalParameterization *local_parameterization)
 Set the (local) parameterisation of a parameter block. More...
 
Getters
std::shared_ptr
< okvis::ceres::ParameterBlock
parameterBlockPtr (uint64_t parameterBlockId)
 Get a shared pointer to a parameter block. More...
 
std::shared_ptr< const
okvis::ceres::ParameterBlock
parameterBlockPtr (uint64_t parameterBlockId) const
 Get a shared pointer to a parameter block. More...
 
std::shared_ptr
< okvis::ceres::ErrorInterface
errorInterfacePtr (::ceres::ResidualBlockId residualBlockId)
 Get a shared pointer to an error term. More...
 
std::shared_ptr< const
okvis::ceres::ErrorInterface
errorInterfacePtr (::ceres::ResidualBlockId residualBlockId) const
 Get a shared pointer to an error term. More...
 
ResidualBlockCollection residuals (uint64_t parameterBlockId) const
 Get the residual blocks of a parameter block. More...
 
ParameterBlockCollection parameters (::ceres::ResidualBlockId residualBlockId) const
 Get the parameters of a residual block. More...
 

Public Attributes

::ceres::Solver::Options options
 Ceres options. More...
 
::ceres::Solver::Summary summary
 Ceres optimization summary. More...
 

Protected Types

typedef
std::unordered_multimap
< uint64_t, ResidualBlockSpec
Id2ResidualBlock_Multimap
 Go from Id to residual block pointer. More...
 
typedef std::unordered_map
< ::ceres::ResidualBlockId,
ParameterBlockCollection
ResidualBlockId2ParameterBlockCollection_Map
 Go from residual block id to its parameter blocks. More...
 

Protected Attributes

uint64_t residualCounter_
 count the inserted residual blocks. More...
 
std::shared_ptr< ::ceres::Problem > problem_
 The ceres problem. More...
 
Id2ParameterBlock_Map id2ParameterBlock_Map_
 The map connecting parameter block ID's and parameter blocks. More...
 
ResidualBlockId2ResidualBlockSpec_Map residualBlockId2ResidualBlockSpec_Map_
 Go from residual ID to specs. More...
 
Id2ResidualBlock_Multimap id2ResidualBlock_Multimap_
 Go from Id to residual block pointer. More...
 
ResidualBlockId2ParameterBlockCollection_Map residualBlockId2ParameterBlockCollection_Map_
 Go from residual block id to its parameter blocks. More...
 
okvis::ceres::HomogeneousPointLocalParameterization homogeneousPointLocalParameterization_
 Store parameterisation locally. More...
 
okvis::ceres::PoseLocalParameterization poseLocalParameterization_
 Store parameterisation locally. More...
 
okvis::ceres::PoseLocalParameterization2d poseLocalParameterization2d_
 Store parameterisation locally. More...
 
okvis::ceres::PoseLocalParameterization3d poseLocalParameterization3d_
 Store parameterisation locally. More...
 
okvis::ceres::PoseLocalParameterization4d poseLocalParameterization4d_
 Store parameterisation locally. More...
 

Detailed Description

The Map class. This keeps track of how parameter blocks are connected to residual blocks. In essence, it encapsulates the ceres::Problem. This way, we can easily manipulate the optimisation problem. You could argue why not use cere's internal mechanisms to do that. We found that our implementation was faster...

Member Typedef Documentation

typedef std::unordered_map<uint64_t, std::shared_ptr<okvis::ceres::ParameterBlock> > okvis::ceres::Map::Id2ParameterBlock_Map

The actual map from Id to parameter block pointer.

typedef std::unordered_multimap<uint64_t, ResidualBlockSpec> okvis::ceres::Map::Id2ResidualBlock_Multimap
protected

Go from Id to residual block pointer.

typedef std::pair<uint64_t, std::shared_ptr<okvis::ceres::ParameterBlock> > okvis::ceres::Map::ParameterBlockSpec
typedef std::unordered_map< ::ceres::ResidualBlockId, ParameterBlockCollection> okvis::ceres::Map::ResidualBlockId2ParameterBlockCollection_Map
protected

Go from residual block id to its parameter blocks.

typedef std::unordered_map< ::ceres::ResidualBlockId, ResidualBlockSpec> okvis::ceres::Map::ResidualBlockId2ResidualBlockSpec_Map

The actual map from Id to residual block specs.

Member Enumeration Documentation

The Parameterisation enum.

Enumerator
HomogeneousPoint 

Use okvis::ceres::HomogeneousPointLocalParameterization.

Pose6d 

Use okvis::ceres::PoseLocalParameterization.

Pose3d 

Use okvis::ceres::PoseLocalParameterization3d (orientation varying).

Pose4d 

Use okvis::ceres::PoseLocalParameterization4d (position and yaw varying).

Pose2d 

Use okvis::ceres::PoseLocalParameterization2d (roll/pitch varying).

Trivial 

No local parameterisation.

Constructor & Destructor Documentation

okvis::ceres::Map::Map ( )

Constructor.

Member Function Documentation

bool okvis::ceres::Map::addParameterBlock ( std::shared_ptr< okvis::ceres::ParameterBlock parameterBlock,
int  parameterization = Parameterization::Trivial,
const int  group = -1 
)

Add a parameter block to the map.

Parameters
parameterBlockParameter block to insert.
parameterizationokvis::ceres::Parameterization to tell how to do the local parameterisation.
groupSchur elimination group – currently unused.
Returns
True if successful.
ceres::ResidualBlockId okvis::ceres::Map::addResidualBlock ( std::shared_ptr< ::ceres::CostFunction >  cost_function,
::ceres::LossFunction *  loss_function,
std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &  parameterBlockPtrs 
)

Adds a residual block.

Parameters
[in]cost_functionThe error term to be used.
[in]loss_functionUse an m-estimator? NULL, if not needed.
[in]parameterBlockPtrsA vector that contains all the parameter blocks the error term relates to.
Returns
ceres::ResidualBlockId okvis::ceres::Map::addResidualBlock ( std::shared_ptr< ::ceres::CostFunction >  cost_function,
::ceres::LossFunction *  loss_function,
std::shared_ptr< okvis::ceres::ParameterBlock x0,
std::shared_ptr< okvis::ceres::ParameterBlock x1 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x2 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x3 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x4 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x5 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x6 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x7 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x8 = std::shared_ptr< okvis::ceres::ParameterBlock>(),
std::shared_ptr< okvis::ceres::ParameterBlock x9 = std::shared_ptr< okvis::ceres::ParameterBlock>() 
)

Add a residual block. See respective ceres docu. If more are needed, see other interface.

Parameters
[in]cost_functionThe error term to be used.
[in]loss_functionUse an m-estimator? NULL, if not needed.
[in]x0The first parameter block.
[in]x1The second parameter block (if existent).
[in]x2The third parameter block (if existent).
[in]x3The 4th parameter block (if existent).
[in]x4The 5th parameter block (if existent).
[in]x5The 6th parameter block (if existent).
[in]x6The 7th parameter block (if existent).
[in]x7The 8th parameter block (if existent).
[in]x8The 9th parameter block (if existent).
[in]x9The 10th parameter block (if existent).
Returns
The residual block ID, i.e. what cost_function points to.
std::shared_ptr< okvis::ceres::ErrorInterface > okvis::ceres::Map::errorInterfacePtr ( ::ceres::ResidualBlockId  residualBlockId)

Get a shared pointer to an error term.

std::shared_ptr< const okvis::ceres::ErrorInterface > okvis::ceres::Map::errorInterfacePtr ( ::ceres::ResidualBlockId  residualBlockId) const

Get a shared pointer to an error term.

void okvis::ceres::Map::getLhs ( uint64_t  parameterBlockId,
Eigen::MatrixXd &  H 
)

Obtain the Hessian block for a specific parameter block.

Parameters
[in]parameterBlockIdParameter block ID of interest.
[out]Hthe output Hessian block.
const Id2ParameterBlock_Map& okvis::ceres::Map::id2parameterBlockMap ( ) const
inline

Get map connecting parameter block IDs to parameter blocks.

bool okvis::ceres::Map::isJacobianCorrect ( ::ceres::ResidualBlockId  residualBlockId,
double  relTol = 1e-6 
) const

Check a Jacobian with numeric differences.

Warning
Checks the minimal version only.
Parameters
[in]residualBlockIdThe ID of the residual block to be checked.
[in]relTolRelative numeric tolerance.
Returns
True if correct.
bool okvis::ceres::Map::parameterBlockExists ( uint64_t  parameterBlockId) const

Check whether a certain parameter block is part of the map.

Parameters
parameterBlockIdID of parameter block to find.
Returns
True if parameter block is part of map.
std::shared_ptr< okvis::ceres::ParameterBlock > okvis::ceres::Map::parameterBlockPtr ( uint64_t  parameterBlockId)

Get a shared pointer to a parameter block.

std::shared_ptr< const okvis::ceres::ParameterBlock > okvis::ceres::Map::parameterBlockPtr ( uint64_t  parameterBlockId) const

Get a shared pointer to a parameter block.

Map::ParameterBlockCollection okvis::ceres::Map::parameters ( ::ceres::ResidualBlockId  residualBlockId) const

Get the parameters of a residual block.

Parameters
[in]residualBlockIdThe ID of the residual block in question.
Returns
Infos about all the parameter blocks connected.
void okvis::ceres::Map::printParameterBlockInfo ( uint64_t  parameterBlockId) const

Log information on a parameter block.

void okvis::ceres::Map::printResidualBlockInfo ( ::ceres::ResidualBlockId  residualBlockId) const

Log information on a residual block.

bool okvis::ceres::Map::removeParameterBlock ( uint64_t  parameterBlockId)

Remove a parameter block from the map.

Parameters
parameterBlockIdID of block to remove.
Returns
True if successful.
bool okvis::ceres::Map::removeParameterBlock ( std::shared_ptr< okvis::ceres::ParameterBlock parameterBlock)

Remove a parameter block from the map.

Parameters
parameterBlockPointer to the block to remove.
Returns
True if successful.
bool okvis::ceres::Map::removeResidualBlock ( ::ceres::ResidualBlockId  id)

Remove a residual block.

Parameters
[in]idThe residual block ID of the residual block to be removed.
Returns
True on success.
bool okvis::ceres::Map::resetParameterization ( uint64_t  parameterBlockId,
int  parameterization 
)

Reset the (local) parameterisation of a parameter block.

Parameters
[in]parameterBlockIdThe ID of the parameter block in question.
[in]parameterizationokvis::ceres::Parameterization to tell how to do the local parameterisation.
Returns
True on success.
void okvis::ceres::Map::resetResidualBlock ( ::ceres::ResidualBlockId  residualBlockId,
std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &  parameterBlockPtrs 
)

Replace the parameters connected to a residual block ID.

Parameters
[in]residualBlockIdThe ID of the residual block the parameter blocks of which are to be to be replaced.
[in]parameterBlockPtrsA vector containing the parameter blocks to be replaced.
const ResidualBlockId2ResidualBlockSpec_Map& okvis::ceres::Map::residualBlockId2ResidualBlockSpecMap ( ) const
inline

Get the actual map from Id to residual block specs.

Map::ResidualBlockCollection okvis::ceres::Map::residuals ( uint64_t  parameterBlockId) const

Get the residual blocks of a parameter block.

Parameters
[in]parameterBlockIdThe ID of the parameter block in question.
Returns
Infos about all the residual blocks connected.
bool okvis::ceres::Map::setParameterBlockConstant ( uint64_t  parameterBlockId)

Do not optimise a certain parameter block.

Parameters
[in]parameterBlockIdThe parameter block ID of the parameter block to set fixed.
Returns
True on success.
bool okvis::ceres::Map::setParameterBlockConstant ( std::shared_ptr< okvis::ceres::ParameterBlock parameterBlock)
inline

Do not optimise a certain parameter block.

Parameters
[in]parameterBlockPointer to the parameter block that should be constant.
Returns
True on success.
bool okvis::ceres::Map::setParameterBlockVariable ( uint64_t  parameterBlockId)

Optimise a certain parameter block (this is the default).

Parameters
[in]parameterBlockIdThe parameter block ID of the parameter block to set fixed.
Returns
True on success.
bool okvis::ceres::Map::setParameterBlockVariable ( std::shared_ptr< okvis::ceres::ParameterBlock parameterBlock)
inline

Optimise a certain parameter block (this is the default).

Parameters
[in]parameterBlockPointer to the parameter block that should be optimised.
Returns
True on success.
bool okvis::ceres::Map::setParameterization ( uint64_t  parameterBlockId,
::ceres::LocalParameterization *  local_parameterization 
)

Set the (local) parameterisation of a parameter block.

Parameters
[in]parameterBlockIdThe ID of the parameter block in question.
[in]local_parameterizationGive it an actual local parameterisation object.
Returns
True on success.
bool okvis::ceres::Map::setParameterization ( std::shared_ptr< okvis::ceres::ParameterBlock parameterBlock,
::ceres::LocalParameterization *  local_parameterization 
)
inline

Set the (local) parameterisation of a parameter block.

Parameters
[in]parameterBlockThe pointer to the parameter block in question.
[in]local_parameterizationGive it an actual local parameterisation object.
Returns
True on success.
void okvis::ceres::Map::solve ( )
inline

Solve the optimization problem.

Member Data Documentation

okvis::ceres::HomogeneousPointLocalParameterization okvis::ceres::Map::homogeneousPointLocalParameterization_
protected

Store parameterisation locally.

Id2ParameterBlock_Map okvis::ceres::Map::id2ParameterBlock_Map_
protected

The map connecting parameter block ID's and parameter blocks.

Id2ResidualBlock_Multimap okvis::ceres::Map::id2ResidualBlock_Multimap_
protected

Go from Id to residual block pointer.

::ceres::Solver::Options okvis::ceres::Map::options

Ceres options.

okvis::ceres::PoseLocalParameterization2d okvis::ceres::Map::poseLocalParameterization2d_
protected

Store parameterisation locally.

okvis::ceres::PoseLocalParameterization3d okvis::ceres::Map::poseLocalParameterization3d_
protected

Store parameterisation locally.

okvis::ceres::PoseLocalParameterization4d okvis::ceres::Map::poseLocalParameterization4d_
protected

Store parameterisation locally.

okvis::ceres::PoseLocalParameterization okvis::ceres::Map::poseLocalParameterization_
protected

Store parameterisation locally.

std::shared_ptr< ::ceres::Problem> okvis::ceres::Map::problem_
protected

The ceres problem.

ResidualBlockId2ParameterBlockCollection_Map okvis::ceres::Map::residualBlockId2ParameterBlockCollection_Map_
protected

Go from residual block id to its parameter blocks.

ResidualBlockId2ResidualBlockSpec_Map okvis::ceres::Map::residualBlockId2ResidualBlockSpec_Map_
protected

Go from residual ID to specs.

uint64_t okvis::ceres::Map::residualCounter_
protected

count the inserted residual blocks.

::ceres::Solver::Summary okvis::ceres::Map::summary

Ceres optimization summary.


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