An optimizer that uses Angular Cost function on the unit sphere for Bundle Adjustment and analytical jacobian derivation. More...

#include <AngularAdjustmentCERESAnalytic.h>

Inheritance diagram for isae::AngularAdjustmentCERESAnalytic:

[legend]

Collaboration diagram for isae::AngularAdjustmentCERESAnalytic:

[legend]

Public Member Functions
	AngularAdjustmentCERESAnalytic ()=default

	~AngularAdjustmentCERESAnalytic ()=default

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

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

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

Public Member Functions inherited from isae::AOptimizer
	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...

Protected Member Functions
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) override
	Add visual residuals to a CERES problem for a local map. More...

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

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

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

Protected Member Functions inherited from isae::AOptimizer
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...

Additional Inherited Members
Protected Attributes inherited from isae::AOptimizer
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

An optimizer that uses Angular Cost function on the unit sphere for Bundle Adjustment and analytical jacobian derivation.

Constructor & Destructor Documentation

◆ AngularAdjustmentCERESAnalytic()

isae::AngularAdjustmentCERESAnalytic::AngularAdjustmentCERESAnalytic ( )

default

◆ ~AngularAdjustmentCERESAnalytic()

isae::AngularAdjustmentCERESAnalytic::~AngularAdjustmentCERESAnalytic ( )

default

Member Function Documentation

◆ addLandmarkResiduals()

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

overrideprotectedvirtual

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.

Implements isae::AOptimizer.

◆ addMarginalizationResiduals()

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

overrideprotectedvirtual

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.

CASE 1 VIO ///

CASE 2 VO ///

Reimplemented from isae::AOptimizer.

◆ addResidualsLocalMap()

uint isae::AngularAdjustmentCERESAnalytic::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
	)

overrideprotectedvirtual

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.

Implements isae::AOptimizer.

◆ addSingleFrameResiduals()

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

overrideprotectedvirtual

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.

Implements isae::AOptimizer.

◆ landmarkOptimizationNoFov()

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

overridevirtual

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 from isae::AOptimizer.

◆ marginalize()

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

overridevirtual

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 from isae::AOptimizer.

◆ marginalizeRelative()

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

overridevirtual

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 from isae::AOptimizer.

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

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

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ AngularAdjustmentCERESAnalytic()

◆ ~AngularAdjustmentCERESAnalytic()

Member Function Documentation

◆ addLandmarkResiduals()

◆ addMarginalizationResiduals()

◆ addResidualsLocalMap()

◆ addSingleFrameResiduals()

◆ landmarkOptimizationNoFov()

◆ marginalize()

◆ marginalizeRelative()