Abstract class for optimization pipeline based on CERES. More...

#include <AOptimizer.h>

Inheritance diagram for isae::AOptimizer:

[legend]

Public Member Functions
	AOptimizer ()

void	resetMarginalization ()

bool	landmarkOptimization (std::shared_ptr< Frame > &frame)
	Structure only Bundle Adjustment for a frame. More...

bool	singleFrameOptimization (std::shared_ptr< Frame > &moving_frame)
	Motion only Bundle Adjustment for a frame. More...

bool	singleFrameVIOptimization (std::shared_ptr< isae::Frame > &moving_frame)
	Visual-Inertial Motion only Bundle Adjustment for a frame. More...

bool	localMapBA (std::shared_ptr< isae::LocalMap > &local_map, const size_t fixed_frame_number=0)
	Bundle Adjustment for a local map. More...

bool	localMapVIOptimization (std::shared_ptr< isae::LocalMap > &local_map, const size_t fixed_frame_number=0)
	Visual-Inertial Bundle Adjustment for a local map. More...

double	VIInit (std::shared_ptr< isae::LocalMap > &local_map, Eigen::Matrix3d &R_w_i, bool optim_scale=false)
	Visual-Inertial Initialization for a local map. More...

virtual bool	landmarkOptimizationNoFov (std::shared_ptr< Frame > &f, std::shared_ptr< Frame > &fp, Eigen::Affine3d &T_cam0_cam0p, double info_scale)
	Structure only Bundle Adjustment for a frame with non overlapping fields of view sensors. More...

virtual bool	marginalize (std::shared_ptr< Frame > &frame0, std::shared_ptr< Frame > &frame1, bool enable_sparsif)
	Marginalization of a frame and its landmarks. More...

virtual Eigen::MatrixXd	marginalizeRelative (std::shared_ptr< Frame > &frame0, std::shared_ptr< Frame > &frame1)
	Marginalization of all the landmarks linked to two frame and computation of the information matrix of the relative pose. More...

Protected Member Functions
virtual uint	addResidualsLocalMap (ceres::Problem &problem, ceres::LossFunction loss_function, ceres::ParameterBlockOrdering ordering, std::vector< std::shared_ptr< Frame >> &frame_vector, size_t fixed_frame_number, std::shared_ptr< isae::LocalMap > &local_map)=0
	Add visual residuals to a CERES problem for a local map. More...

virtual uint	addLandmarkResiduals (ceres::Problem &problem, ceres::LossFunction *loss_function, typed_vec_landmarks &cloud_to_optimize)=0
	Add visual residuals to a CERES problem for structure only BA. More...

virtual uint	addSingleFrameResiduals (ceres::Problem &problem, ceres::LossFunction *loss_function, std::shared_ptr< Frame > &frame, typed_vec_landmarks &cloud_to_optimize)=0
	Add visual residuals to a CERES problem for a motion only BA. More...

virtual uint	addMarginalizationResiduals (ceres::Problem &problem, ceres::LossFunction loss_function, ceres::ParameterBlockOrdering ordering)
	Add marginalization residuals to a CERES problem. More...

uint	addIMUResiduals (ceres::Problem &problem, ceres::LossFunction loss_function, ceres::ParameterBlockOrdering ordering, std::vector< std::shared_ptr< Frame >> &frame_vector, size_t fixed_frame_number)
	Add IMU residuals to a CERES problem. More...

Protected Attributes
std::unordered_map< std::shared_ptr< Frame >, PoseParametersBlock >	_map_frame_posepar
	map for pose parameters More...

std::unordered_map< std::shared_ptr< Frame >, PointXYZParametersBlock >	_map_frame_velpar
	map for velocity parameters More...

std::unordered_map< std::shared_ptr< Frame >, PointXYZParametersBlock >	_map_frame_dbapar
	map for accelerometer bias parameters More...

std::unordered_map< std::shared_ptr< Frame >, PointXYZParametersBlock >	_map_frame_dbgpar
	map for gyroscope bias parameters More...

std::unordered_map< std::shared_ptr< ALandmark >, PointXYZParametersBlock >	_map_lmk_ptpar
	map for landmark point parameters More...

std::unordered_map< std::shared_ptr< ALandmark >, PoseParametersBlock >	_map_lmk_posepar
	map for landmark pose parameters More...

bool	_enable_sparsif = false
	enable sparsification of the marginalization More...

std::shared_ptr< Marginalization >	_marginalization
	marginalization object More...

std::shared_ptr< Marginalization >	_marginalization_last
	marginalization object of the last optimization More...

Detailed Description

Abstract class for optimization pipeline based on CERES.

This class provides methods for optimizing frames, landmarks, and local maps. It also implements methods involving factors such as initialization and marginalization. This virtual formulation allows for different optimization scheme with different cost functions, different parameterization or different CERES settings (e.g. numerical or analytic Jacobians).

Constructor & Destructor Documentation

◆ AOptimizer()

isae::AOptimizer::AOptimizer ( )

inline

Member Function Documentation

◆ addIMUResiduals()

uint isae::AOptimizer::addIMUResiduals	(	ceres::Problem &	problem,
		ceres::LossFunction *	loss_function,
		ceres::ParameterBlockOrdering *	ordering,
		std::vector< std::shared_ptr< Frame >> &	frame_vector,
		size_t	fixed_frame_number
	)

protected

Add IMU residuals to a CERES problem.

Parameters

problem	The CERES problem to which the residuals will be added.
loss_function	The loss function to be used for the residuals.
ordering	The parameter block ordering for the problem.
frame_vector	The vector of frames in the local map.
fixed_frame_number	The number of frames to fix during optimization.

◆ addLandmarkResiduals()

virtual uint isae::AOptimizer::addLandmarkResiduals	(	ceres::Problem &	problem,
		ceres::LossFunction *	loss_function,
		typed_vec_landmarks &	cloud_to_optimize
	)

protectedpure virtual

Add visual residuals to a CERES problem for structure only BA.

Parameters

problem	The CERES problem to which the residuals will be added.
loss_function	The loss function to be used for the residuals.
cloud_to_optimize	The landmarks to be optimized.

Returns: The number of residuals added.

Implemented in isae::AngularAdjustmentCERESAnalytic, isae::BundleAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESNumeric.

◆ addMarginalizationResiduals()

virtual uint isae::AOptimizer::addMarginalizationResiduals	(	ceres::Problem &	problem,
		ceres::LossFunction *	loss_function,
		ceres::ParameterBlockOrdering *	ordering
	)

inlineprotectedvirtual

Add marginalization residuals to a CERES problem.

Parameters

problem	The CERES problem to which the residuals will be added.
loss_function	The loss function to be used for the residuals.
ordering	The parameter block ordering for the problem.

Returns: The number of marginalization residuals added.

Reimplemented in isae::AngularAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESAnalytic.

◆ addResidualsLocalMap()

virtual uint isae::AOptimizer::addResidualsLocalMap	(	ceres::Problem &	problem,
		ceres::LossFunction *	loss_function,
		ceres::ParameterBlockOrdering *	ordering,
		std::vector< std::shared_ptr< Frame >> &	frame_vector,
		size_t	fixed_frame_number,
		std::shared_ptr< isae::LocalMap > &	local_map
	)

protectedpure virtual

Add visual residuals to a CERES problem for a local map.

Parameters

problem	The CERES problem to which the residuals will be added.
loss_function	The loss function to be used for the residuals.
ordering	The parameter block ordering for the problem.
frame_vector	The vector of frames in the local map.
fixed_frame_number	The number of frames to fix during optimization.
local_map	The local map to which the residuals will be added.

Returns: The number of residuals added.

Implemented in isae::AngularAdjustmentCERESAnalytic, isae::BundleAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESNumeric.

◆ addSingleFrameResiduals()

virtual uint isae::AOptimizer::addSingleFrameResiduals	(	ceres::Problem &	problem,
		ceres::LossFunction *	loss_function,
		std::shared_ptr< Frame > &	frame,
		typed_vec_landmarks &	cloud_to_optimize
	)

protectedpure virtual

Add visual residuals to a CERES problem for a motion only BA.

Parameters

problem	The CERES problem to which the residuals will be added.
loss_function	The loss function to be used for the residuals.
frame	The frame to which the residuals will be added.
cloud_to_optimize	The landmarks belonging to the frame.

Returns: The number of residuals added.

Implemented in isae::AngularAdjustmentCERESAnalytic, isae::BundleAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESNumeric.

◆ landmarkOptimization()

bool isae::AOptimizer::landmarkOptimization ( std::shared_ptr< Frame > & frame )

Structure only Bundle Adjustment for a frame.

Parameters

frame The frame to optimize.

This method optimize the landmarks associated to the frame, setting static the pose parameters. It can be used to refine the triangulation of new landmarks.

◆ landmarkOptimizationNoFov()

bool isae::AOptimizer::landmarkOptimizationNoFov	(	std::shared_ptr< Frame > &	f,
		std::shared_ptr< Frame > &	fp,
		Eigen::Affine3d &	T_cam0_cam0p,
		double	info_scale
	)

virtual

Structure only Bundle Adjustment for a frame with non overlapping fields of view sensors.

Parameters

f	The frame to optimize.
fp	The previous frame to optimize.
T_cam0_cam0p	The transformation from the current frame to the previous frame.
info_scale	The scale factor for the information matrix.

Reimplemented in isae::AngularAdjustmentCERESAnalytic.

◆ localMapBA()

bool isae::AOptimizer::localMapBA	(	std::shared_ptr< isae::LocalMap > &	local_map,
		const size_t	fixed_frame_number = `0`
	)

Bundle Adjustment for a local map.

Parameters

local_map	The local map to optimize.
fixed_frame_number	The frame number to fix during optimization (default is 0).

◆ localMapVIOptimization()

bool isae::AOptimizer::localMapVIOptimization	(	std::shared_ptr< isae::LocalMap > &	local_map,
		const size_t	fixed_frame_number = `0`
	)

Visual-Inertial Bundle Adjustment for a local map.

Parameters

local_map	The local map to optimize.
fixed_frame_number	The frame number to fix during optimization (default is 0).

◆ marginalize()

virtual bool isae::AOptimizer::marginalize	(	std::shared_ptr< Frame > &	frame0,
		std::shared_ptr< Frame > &	frame1,
		bool	enable_sparsif
	)

inlinevirtual

Marginalization of a frame and its landmarks.

Parameters

frame0	The frame to marginalize.
frame1	The frame to keep connected to frame0.
enable_sparsif	Whether to enable sparsification (default is false).

Perform all the marginalization steps for a given frame. A marginalization object deals with all these steps

Reimplemented in isae::AngularAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESAnalytic.

◆ marginalizeRelative()

virtual Eigen::MatrixXd isae::AOptimizer::marginalizeRelative	(	std::shared_ptr< Frame > &	frame0,
		std::shared_ptr< Frame > &	frame1
	)

inlinevirtual

Marginalization of all the landmarks linked to two frame and computation of the information matrix of the relative pose.

Parameters

frame0	The first frame to marginalize.
frame1	The second frame to marginalize.

Returns: The information matrix of the relative pose between frame0 and frame1.

This method computes the information matrix of the relative pose between two frames and marginalizes the landmarks linked to them. It uses NFR to extract a relative pose factor and to derive its information matrix.

Reimplemented in isae::AngularAdjustmentCERESAnalytic, and isae::BundleAdjustmentCERESAnalytic.

◆ resetMarginalization()

void isae::AOptimizer::resetMarginalization ( )

inline

◆ singleFrameOptimization()

bool isae::AOptimizer::singleFrameOptimization ( std::shared_ptr< Frame > & moving_frame )

Motion only Bundle Adjustment for a frame.

Parameters

moving_frame The frame to optimize.

This method optimizes the frame's pose parameters only, setting static the landmarks.

◆ singleFrameVIOptimization()

bool isae::AOptimizer::singleFrameVIOptimization ( std::shared_ptr< isae::Frame > & moving_frame )

Visual-Inertial Motion only Bundle Adjustment for a frame.

Parameters

moving_frame The frame to optimize.

This method optimizes the frame's pose and velocity parameters.

◆ VIInit()

double isae::AOptimizer::VIInit	(	std::shared_ptr< isae::LocalMap > &	local_map,
		Eigen::Matrix3d &	R_w_i,
		bool	optim_scale = `false`
	)

Visual-Inertial Initialization for a local map.

Parameters

local_map	The local map to initialize.
R_w_i	The initial rotation of the world frame with respect to the inertial frame.
optim_scale	Whether to optimize the scale (default is false).

Returns: The scale factor used for initialization.

Member Data Documentation

◆ _enable_sparsif

bool isae::AOptimizer::_enable_sparsif = false

protected

enable sparsification of the marginalization

◆ _map_frame_dbapar

std::unordered_map<std::shared_ptr<Frame>, PointXYZParametersBlock> isae::AOptimizer::_map_frame_dbapar

protected

map for accelerometer bias parameters

◆ _map_frame_dbgpar

std::unordered_map<std::shared_ptr<Frame>, PointXYZParametersBlock> isae::AOptimizer::_map_frame_dbgpar

protected

map for gyroscope bias parameters

◆ _map_frame_posepar

std::unordered_map<std::shared_ptr<Frame>, PoseParametersBlock> isae::AOptimizer::_map_frame_posepar

protected

map for pose parameters

◆ _map_frame_velpar

std::unordered_map<std::shared_ptr<Frame>, PointXYZParametersBlock> isae::AOptimizer::_map_frame_velpar

protected

map for velocity parameters

◆ _map_lmk_posepar

std::unordered_map<std::shared_ptr<ALandmark>, PoseParametersBlock> isae::AOptimizer::_map_lmk_posepar

protected

map for landmark pose parameters

◆ _map_lmk_ptpar

std::unordered_map<std::shared_ptr<ALandmark>, PointXYZParametersBlock> isae::AOptimizer::_map_lmk_ptpar

protected

map for landmark point parameters

◆ _marginalization

std::shared_ptr<Marginalization> isae::AOptimizer::_marginalization

protected

marginalization object

◆ _marginalization_last

std::shared_ptr<Marginalization> isae::AOptimizer::_marginalization_last

protected

marginalization object of the last optimization

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

/home/deos/ce.debeunne/colcon_ws/src/SaDVIO/cpp/include/isaeslam/optimizers/AOptimizer.h
/home/deos/ce.debeunne/colcon_ws/src/SaDVIO/cpp/src/optimizers/AOptimizer.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ AOptimizer()

Member Function Documentation

◆ addIMUResiduals()

◆ addLandmarkResiduals()

◆ addMarginalizationResiduals()

◆ addResidualsLocalMap()

◆ addSingleFrameResiduals()

◆ landmarkOptimization()

◆ landmarkOptimizationNoFov()

◆ localMapBA()

◆ localMapVIOptimization()

◆ marginalize()

◆ marginalizeRelative()

◆ resetMarginalization()

◆ singleFrameOptimization()

◆ singleFrameVIOptimization()

◆ VIInit()

Member Data Documentation

◆ _enable_sparsif

◆ _map_frame_dbapar

◆ _map_frame_dbgpar

◆ _map_frame_posepar

◆ _map_frame_velpar

◆ _map_lmk_posepar

◆ _map_lmk_ptpar

◆ _marginalization

◆ _marginalization_last