#include <OptimizerLMBatch.hpp>

Inheritance diagram for holistic_fusion::OptimizerLMBatch:

[legend]

Collaboration diagram for holistic_fusion::OptimizerLMBatch:

[legend]

Public Member Functions
	OptimizerLMBatch (const std::shared_ptr< GraphConfig > graphConfigPtr)
	~OptimizerLMBatch ()=default
bool	update () override
bool	update (const gtsam::NonlinearFactorGraph &newGraphFactors, const gtsam::Values &newGraphValues, const std::map< gtsam::Key, double > &newGraphKeysTimeStampMap, const int depth=0) override
bool	updateExistingValues (const gtsam::Values &newGraphValues) override
void	optimize (int maxIterations) override
const gtsam::Values &	getAllOptimizedStates () override
gtsam::KeyVector	getAllOptimizedKeys () override
const gtsam::NonlinearFactorGraph &	getNonlinearFactorGraph () const override
const std::map< gtsam::Key, double > &	getFullKeyTimestampMap () override
gtsam::Pose3	calculateEstimatedPose3 (const gtsam::Key &key) override
gtsam::Vector3	calculateEstimatedVelocity3 (const gtsam::Key &key) override
gtsam::imuBias::ConstantBias	calculateEstimatedBias (const gtsam::Key &key) override
gtsam::Point3	calculateEstimatedPoint3 (const gtsam::Key &key) override
gtsam::Vector	calculateEstimatedVector (const gtsam::Key &key) override
void	divideGraphIntoSubGraphs ()
gtsam::Matrix	calculateMarginalCovarianceMatrixAtKey (const gtsam::Key &valueKey) override
Public Member Functions inherited from holistic_fusion::OptimizerLM
	OptimizerLM (const std::shared_ptr< GraphConfig > graphConfigPtr)
	~OptimizerLM ()=default
Public Member Functions inherited from holistic_fusion::OptimizerBase
	OptimizerBase (const std::shared_ptr< GraphConfig > graphConfigPtr)
	~OptimizerBase ()=default
virtual void	addLatestImuBiasBelief (const gtsam::imuBias::ConstantBias &imuBias)
virtual void	addLatestPoseBelief (const gtsam::Pose3 &pose)

Static Public Member Functions
static std::vector< double >	linspace (double start, double end, int num)
static int	filterFactorsConnectedToKey (gtsam::NonlinearFactorGraph &graph, const gtsam::Key &keyToFilter)
static gtsam::Marginals	computeMarginals (gtsam::NonlinearFactorGraph &graph, gtsam::Values &values)

Private Attributes
std::shared_ptr< gtsam::LevenbergMarquardtOptimizer >	batchSmootherPtr_
gtsam::NonlinearFactorGraph	containerBatchSmootherFactors_
gtsam::Values	containerBatchSmootherValues_
bool	optimizedAtLeastOnceFlag_ = false
gtsam::Values	valuesLastOptimizedResult_
std::map< gtsam::Key, double >	batchSmootherKeyTimestampMap_
bool	dividedIntoSubGraphsFlag_ = false
std::vector< gtsam::NonlinearFactorGraph >	subGraphs_
std::unordered_map< gtsam::Key, int >	keyToSubGraphIndexMap_
std::vector< gtsam::Marginals >	marginalsForSubGraphs_
gtsam::NonlinearFactorGraph	graphLastOptimizedResult_
bool	marginalsComputedForLastOptimizedResultFlag_ = false
gtsam::Marginals	marginalsForLastOptimizedResult_
int	currentOptimizedSubgraphIndex_ = -1

Additional Inherited Members
Protected Attributes inherited from holistic_fusion::OptimizerLM
gtsam::LevenbergMarquardtParams	lmParams_
Protected Attributes inherited from holistic_fusion::OptimizerBase
std::shared_ptr< GraphConfig >	graphConfigPtr_
gtsam::imuBias::ConstantBias	latestImuBias_
gtsam::Pose3	latestPose_

Detailed Description

Definition at line 19 of file OptimizerLMBatch.hpp.

Constructor & Destructor Documentation

◆ OptimizerLMBatch()

holistic_fusion::OptimizerLMBatch::OptimizerLMBatch ( const std::shared_ptr< GraphConfig > graphConfigPtr )

inlineexplicit

Definition at line 21 of file OptimizerLMBatch.hpp.

◆ ~OptimizerLMBatch()

holistic_fusion::OptimizerLMBatch::~OptimizerLMBatch ( )

default

Member Function Documentation

◆ calculateEstimatedBias()

gtsam::imuBias::ConstantBias holistic_fusion::OptimizerLMBatch::calculateEstimatedBias ( const gtsam::Key & key )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 137 of file OptimizerLMBatch.hpp.

◆ calculateEstimatedPoint3()

gtsam::Point3 holistic_fusion::OptimizerLMBatch::calculateEstimatedPoint3 ( const gtsam::Key & key )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 144 of file OptimizerLMBatch.hpp.

◆ calculateEstimatedPose3()

gtsam::Pose3 holistic_fusion::OptimizerLMBatch::calculateEstimatedPose3 ( const gtsam::Key & key )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 123 of file OptimizerLMBatch.hpp.

◆ calculateEstimatedVector()

gtsam::Vector holistic_fusion::OptimizerLMBatch::calculateEstimatedVector ( const gtsam::Key & key )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 151 of file OptimizerLMBatch.hpp.

◆ calculateEstimatedVelocity3()

gtsam::Vector3 holistic_fusion::OptimizerLMBatch::calculateEstimatedVelocity3 ( const gtsam::Key & key )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 130 of file OptimizerLMBatch.hpp.

◆ calculateMarginalCovarianceMatrixAtKey()

gtsam::Matrix holistic_fusion::OptimizerLMBatch::calculateMarginalCovarianceMatrixAtKey ( const gtsam::Key & valueKey )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 388 of file OptimizerLMBatch.hpp.

◆ computeMarginals()

gtsam::Marginals holistic_fusion::OptimizerLMBatch::computeMarginals	(	gtsam::NonlinearFactorGraph &	graph,
		gtsam::Values &	values )

inlinestatic

Definition at line 195 of file OptimizerLMBatch.hpp.

◆ divideGraphIntoSubGraphs()

void holistic_fusion::OptimizerLMBatch::divideGraphIntoSubGraphs ( )

inline

Definition at line 245 of file OptimizerLMBatch.hpp.

◆ filterFactorsConnectedToKey()

int holistic_fusion::OptimizerLMBatch::filterFactorsConnectedToKey	(	gtsam::NonlinearFactorGraph &	graph,
		const gtsam::Key &	keyToFilter )

inlinestatic

Definition at line 174 of file OptimizerLMBatch.hpp.

◆ getAllOptimizedKeys()

gtsam::KeyVector holistic_fusion::OptimizerLMBatch::getAllOptimizedKeys ( )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 113 of file OptimizerLMBatch.hpp.

◆ getAllOptimizedStates()

const gtsam::Values & holistic_fusion::OptimizerLMBatch::getAllOptimizedStates ( )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 102 of file OptimizerLMBatch.hpp.

◆ getFullKeyTimestampMap()

const std::map< gtsam::Key, double > & holistic_fusion::OptimizerLMBatch::getFullKeyTimestampMap ( )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 119 of file OptimizerLMBatch.hpp.

◆ getNonlinearFactorGraph()

const gtsam::NonlinearFactorGraph & holistic_fusion::OptimizerLMBatch::getNonlinearFactorGraph ( ) const

inlinenodiscardoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 116 of file OptimizerLMBatch.hpp.

◆ linspace()

std::vector< double > holistic_fusion::OptimizerLMBatch::linspace	(	double	start,
		double	end,
		int	num )

inlinestatic

Definition at line 158 of file OptimizerLMBatch.hpp.

◆ optimize()

void holistic_fusion::OptimizerLMBatch::optimize ( int maxIterations )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 74 of file OptimizerLMBatch.hpp.

◆ update() [1/2]

bool holistic_fusion::OptimizerLMBatch::update ( )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 41 of file OptimizerLMBatch.hpp.

◆ update() [2/2]

bool holistic_fusion::OptimizerLMBatch::update	(	const gtsam::NonlinearFactorGraph &	newGraphFactors,
		const gtsam::Values &	newGraphValues,
		const std::map< gtsam::Key, double > &	newGraphKeysTimeStampMap,
		const int	depth = 0 )

inlineoverridevirtual

Implements holistic_fusion::OptimizerBase.

Definition at line 44 of file OptimizerLMBatch.hpp.

◆ updateExistingValues()

bool holistic_fusion::OptimizerLMBatch::updateExistingValues ( const gtsam::Values & newGraphValues )

inlineoverridevirtual

Reimplemented from holistic_fusion::OptimizerLM.

Definition at line 55 of file OptimizerLMBatch.hpp.

Member Data Documentation

◆ batchSmootherKeyTimestampMap_

std::map<gtsam::Key, double> holistic_fusion::OptimizerLMBatch::batchSmootherKeyTimestampMap_

private

Definition at line 445 of file OptimizerLMBatch.hpp.

◆ batchSmootherPtr_

std::shared_ptr<gtsam::LevenbergMarquardtOptimizer> holistic_fusion::OptimizerLMBatch::batchSmootherPtr_

private

Definition at line 438 of file OptimizerLMBatch.hpp.

◆ containerBatchSmootherFactors_

gtsam::NonlinearFactorGraph holistic_fusion::OptimizerLMBatch::containerBatchSmootherFactors_

private

Definition at line 440 of file OptimizerLMBatch.hpp.

◆ containerBatchSmootherValues_

gtsam::Values holistic_fusion::OptimizerLMBatch::containerBatchSmootherValues_

private

Definition at line 441 of file OptimizerLMBatch.hpp.

◆ currentOptimizedSubgraphIndex_

int holistic_fusion::OptimizerLMBatch::currentOptimizedSubgraphIndex_ = -1

private

Definition at line 457 of file OptimizerLMBatch.hpp.

◆ dividedIntoSubGraphsFlag_

bool holistic_fusion::OptimizerLMBatch::dividedIntoSubGraphsFlag_ = false

private

Definition at line 447 of file OptimizerLMBatch.hpp.

◆ graphLastOptimizedResult_

gtsam::NonlinearFactorGraph holistic_fusion::OptimizerLMBatch::graphLastOptimizedResult_

private

Definition at line 452 of file OptimizerLMBatch.hpp.

◆ keyToSubGraphIndexMap_

std::unordered_map<gtsam::Key, int> holistic_fusion::OptimizerLMBatch::keyToSubGraphIndexMap_

private

Definition at line 449 of file OptimizerLMBatch.hpp.

◆ marginalsComputedForLastOptimizedResultFlag_

bool holistic_fusion::OptimizerLMBatch::marginalsComputedForLastOptimizedResultFlag_ = false

private

Definition at line 453 of file OptimizerLMBatch.hpp.

◆ marginalsForLastOptimizedResult_

gtsam::Marginals holistic_fusion::OptimizerLMBatch::marginalsForLastOptimizedResult_

private

Definition at line 454 of file OptimizerLMBatch.hpp.

◆ marginalsForSubGraphs_

std::vector<gtsam::Marginals> holistic_fusion::OptimizerLMBatch::marginalsForSubGraphs_

private

Definition at line 450 of file OptimizerLMBatch.hpp.

◆ optimizedAtLeastOnceFlag_

bool holistic_fusion::OptimizerLMBatch::optimizedAtLeastOnceFlag_ = false

private

Definition at line 443 of file OptimizerLMBatch.hpp.

◆ subGraphs_

std::vector<gtsam::NonlinearFactorGraph> holistic_fusion::OptimizerLMBatch::subGraphs_

private

Definition at line 448 of file OptimizerLMBatch.hpp.

◆ valuesLastOptimizedResult_

gtsam::Values holistic_fusion::OptimizerLMBatch::valuesLastOptimizedResult_

private

Definition at line 444 of file OptimizerLMBatch.hpp.

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

holistic_fusion/include/holistic_fusion/core/optimizer/OptimizerLMBatch.hpp

Public Member Functions

Static Public Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ OptimizerLMBatch()

◆ ~OptimizerLMBatch()

Member Function Documentation

◆ calculateEstimatedBias()

◆ calculateEstimatedPoint3()

◆ calculateEstimatedPose3()

◆ calculateEstimatedVector()

◆ calculateEstimatedVelocity3()

◆ calculateMarginalCovarianceMatrixAtKey()

◆ computeMarginals()

◆ divideGraphIntoSubGraphs()

◆ filterFactorsConnectedToKey()

◆ getAllOptimizedKeys()

◆ getAllOptimizedStates()

◆ getFullKeyTimestampMap()

◆ getNonlinearFactorGraph()

◆ linspace()

◆ optimize()

◆ update() [1/2]

◆ update() [2/2]

◆ updateExistingValues()

Member Data Documentation

◆ batchSmootherKeyTimestampMap_

◆ batchSmootherPtr_

◆ containerBatchSmootherFactors_

◆ containerBatchSmootherValues_

◆ currentOptimizedSubgraphIndex_

◆ dividedIntoSubGraphsFlag_

◆ graphLastOptimizedResult_

◆ keyToSubGraphIndexMap_

◆ marginalsComputedForLastOptimizedResultFlag_

◆ marginalsForLastOptimizedResult_

◆ marginalsForSubGraphs_

◆ optimizedAtLeastOnceFlag_

◆ subGraphs_

◆ valuesLastOptimizedResult_