#include <GraphManager.h>

Collaboration diagram for holistic_fusion::GraphManager:

[legend]

Public Types
typedef gtsam::Key(*	F) (std::uint64_t)

Public Member Functions
	GraphManager (std::shared_ptr< GraphConfig > graphConfigPtr, std::string imuFrame, std::string worldFrame)
	~GraphManager ()
bool	initImuIntegrators (double gravityValue)
bool	initPoseVelocityBiasGraph (double timeStamp, const gtsam::Pose3 &T_W_I0, const gtsam::Pose3 &T_O_I0, const gtsam::Vector3 &imuAngularVelocity)
void	addImuFactorAndGetState (SafeIntegratedNavState &returnPreIntegratedNavState, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &newOptimizedNavStatePtr, const std::shared_ptr< ImuBuffer > &imuBufferPtr, double imuTimeK, bool createNewStateFlag)
bool	getUnaryFactorGeneralKey (gtsam::Key &returnedKey, double &returnedGraphTime, const UnaryMeasurement &unaryMeasurement)
bool	hasReferenceFrameKeyframe (const std::string &worldFrame, const std::string &fixedFrame)
template<class MEASUREMENT_TYPE, int NOISE_DIM, class FACTOR_TYPE, F SYMBOL_SHORTHAND, class... FACTOR_ARGS>
void	addUnaryClassicFactor (const MEASUREMENT_TYPE &unaryMeasurement, const Eigen::Matrix< double, NOISE_DIM, 1 > &unaryNoiseDensity, double measurementTime, const FACTOR_ARGS &... factorArgs)
template<class GMSF_EXPRESSION_TYPE>
void	addUnaryHolisticFactor (const std::shared_ptr< GMSF_EXPRESSION_TYPE > gmsfUnaryExpressionPtr, const bool addToOnlineSmootherFlag=true)
gtsam::Key	addPoseBetweenFactor (const gtsam::Pose3 &deltaPose, const Eigen::Matrix< double, 6, 1 > &poseBetweenNoiseDensity, double lidarTimeKm1, double lidarTimeK, double rate, const RobustNormEnum &robustNormEnum, const double robustNormConstant)
bool	setInitialWorldFrameToFixedFrameTransform (const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
void	updateGraph ()
bool	optimizeSlowBatchSmoother (int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
std::optional< BatchOptimizationResult >	optimizeSlowBatchSmoother (int maxIterations, const bool computePoseCovarianceFlag)
bool	logRealTimeNavStates (const std::string &savePath, const std::string &timeString)
bool	logRealTimeReferenceFrameStates (const std::string &savePath, const std::string &timeString)
bool	logLatencyAndUpdateDuration (const std::string &savePath, const std::string &timeString)
void	saveOptimizedValuesToFile (const gtsam::Values &optimizedValues, const std::map< gtsam::Key, double > &keyTimestampMap, const std::string &savePath, const bool saveCovarianceFlag)
gtsam::NavState	calculateStateAtGeneralKey (bool &computeSuccessfulFlag, const gtsam::Key &generalKey)
Eigen::Vector3d &	getInitAccBiasReference ()
	Getters.
Eigen::Vector3d &	getInitGyrBiasReference ()
const GraphState &	getOptimizedGraphState ()
const gtsam::Key	getPropagatedStateKey ()

Static Public Member Functions
static void	saveOptimizedGraphToG2o (const OptimizerBase &optimizedGraph, const gtsam::Values &optimizedValues, const std::string &saveFileName)

Static Protected Member Functions
static gtsam::NavState	calculateNavStateAtGeneralKey (bool &computeSuccessfulFlag, std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &generalKey, const char *callingFunctionName)
static Eigen::Matrix< double, 6, 6 >	calculatePoseCovarianceAtKeyInWorldFrame (std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &graphKey, const char *callingFunctionName, std::optional< const gtsam::NavState > optionalNavState=std::nullopt)

Private Member Functions
bool	runSlowBatchSmootherOptimization_ (int maxIterations)
BatchOptimizationResult	extractBatchOptimizationResult_ (const bool computePoseCovarianceFlag)
template<class CHILDPTR>
bool	addFactorToRtAndBatchGraph_ (const gtsam::NoiseModelFactor *noiseModelFactorPtr, const bool addToOnlineSmootherFlag=true)
template<class CHILDPTR>
bool	addFactorToRtAndBatchGraph_ (const gtsam::NoiseModelFactor *noiseModelFactorPtr, double measurementTimestamp, const std::string &measurementName, const bool addToOnlineSmootherFlag=true)
template<class CHILDPTR>
bool	addFactorSafelyToRtAndBatchGraph_ (const gtsam::NoiseModelFactor *noiseModelFactorPtr, double measurementTimestamp)
void	updateImuIntegrators_ (const TimeToImuMap &imuMeas)
	Update IMU integrators.
bool	addFactorsToRtSmootherAndOptimize (const gtsam::NonlinearFactorGraph &newRtGraphFactors, const gtsam::Values &newRtGraphValues, const std::map< gtsam::Key, double > &newRtGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr, const int additionalIterations)
bool	addFactorsToBatchSmootherAndOptimize (const gtsam::NonlinearFactorGraph &newBatchGraphFactors, const gtsam::Values &newBatchGraphValues, const std::map< gtsam::Key, double > &newBatchGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr)
bool	findGraphKeys_ (gtsam::Key &closestKeyKm1, gtsam::Key &closestKeyK, double &keyTimeStampDistance, double maxTimestampDistance, double timeKm1, double timeK, const std::string &name)
	Find graph keys for timestamps.
const uint64_t	newPropagatedStateKey_ ()
	Generate new key.
void	writeKeyToKeyTimeStampMap_ (const gtsam::Key &key, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)
	Associate timestamp to each 'value key', e.g. for graph key 0, value keys (x0,v0,b0) need to be associated.
void	writeValueKeysToKeyTimeStampMap_ (const gtsam::Values &values, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)

Private Attributes
std::shared_ptr< TimeGraphKeyBuffer >	timeToKeyBufferPtr_
std::string	imuFrame_
std::string	worldFrame_
DynamicDictionaryContainer	gtsamDynamicExpressionKeys_
FileLogger	fileLogger_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements::Params >	imuParamsPtr_
std::shared_ptr< gtsam::imuBias::ConstantBias >	imuBiasPriorPtr_
holistic_fusion::GraphState	optimizedGraphState_
gtsam::NavState	W_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0))
	Propagated state (at IMU frequency).
gtsam::NavState	O_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0))
Eigen::Isometry3d	T_W_O_ = Eigen::Isometry3d::Identity()
gtsam::Key	propagatedStateKey_ = 0
double	propagatedImuSampleTime_ = 0.0
double	latestGraphStateKeyTime_ = 0.0
double	lastOptimizedStateTime_ = 0.0
gtsam::Vector3	latestGraphStateKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0)
std::shared_ptr< OptimizerBase >	rtOptimizerPtr_
std::shared_ptr< OptimizerBase >	batchOptimizerPtr_
std::shared_ptr< gtsam::NonlinearFactorGraph >	rtFactorGraphBufferPtr_
	Data buffer.
std::shared_ptr< gtsam::NonlinearFactorGraph >	batchFactorGraphBufferPtr_
std::shared_ptr< gtsam::Values >	rtGraphValuesBufferPtr_
std::shared_ptr< gtsam::Values >	batchGraphValuesBufferPtr_
std::shared_ptr< std::map< gtsam::Key, double > >	rtGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< std::map< gtsam::Key, double > >	batchGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements >	imuStepPreintegratorPtr_
	Step Preintegrator.
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements >	imuBufferPreintegratorPtr_
	Buffer Preintegrator.
gtsam::Vector6	lastImuVector_
	IMU Buffer.
std::shared_ptr< holistic_fusion::GraphConfig >	graphConfigPtr_ = nullptr
	Config.
std::map< double, gtsam::Pose3 >	realTimeWorldPoseContainer_ = {}
std::map< double, gtsam::Pose3 >	realTimeOdomPoseContainer_ = {}
std::map< double, TransformsDictionary< Eigen::Isometry3d > >	realTimeReferenceFrameContainer_ = {}
std::set< std::pair< std::string, std::string > >	realTimeReferenceFrameNamePairs_ = {}
std::chrono::time_point< std::chrono::high_resolution_clock >	latencyStartTime_
std::chrono::time_point< std::chrono::high_resolution_clock >	latencyEndTime_
std::map< double, double >	latencyContainer_ = {}
std::chrono::time_point< std::chrono::high_resolution_clock >	updateDurationStartTime_
std::chrono::time_point< std::chrono::high_resolution_clock >	updateDurationEndTime_
std::map< double, double >	updateDurationContainer_ = {}
std::mutex	operateOnGraphDataMutex_
	Mutex.
std::mutex	optimizationRunningMutex_

Detailed Description

GraphManager class.

The GraphManager class is the central class of the HolisticFusion library. It manages the graph, the optimization, and the real-time and batch optimization. It also provides the interface to add measurements to the graph and to retrieve the optimized states.

Definition at line 51 of file GraphManager.h.

Member Typedef Documentation

◆ F

typedef gtsam::Key(* holistic_fusion::GraphManager::F) (std::uint64_t)

Definition at line 83 of file GraphManager.h.

Constructor & Destructor Documentation

◆ GraphManager()

holistic_fusion::GraphManager::GraphManager	(	std::shared_ptr< GraphConfig >	graphConfigPtr,
		std::string	imuFrame,
		std::string	worldFrame )

Definition at line 61 of file GraphManager.cpp.

◆ ~GraphManager()

holistic_fusion::GraphManager::~GraphManager ( )

inline

Definition at line 54 of file GraphManager.h.

Member Function Documentation

◆ addFactorSafelyToRtAndBatchGraph_()

template<class CHILDPTR>

bool holistic_fusion::GraphManager::addFactorSafelyToRtAndBatchGraph_	(	const gtsam::NoiseModelFactor *	noiseModelFactorPtr,
		double	measurementTimestamp )

private

◆ addFactorsToBatchSmootherAndOptimize()

bool holistic_fusion::GraphManager::addFactorsToBatchSmootherAndOptimize	(	const gtsam::NonlinearFactorGraph &	newBatchGraphFactors,
		const gtsam::Values &	newBatchGraphValues,
		const std::map< gtsam::Key, double > &	newBatchGraphKeysTimestampsMap,
		const std::shared_ptr< GraphConfig > &	graphConfigPtr )

private

Definition at line 1362 of file GraphManager.cpp.

◆ addFactorsToRtSmootherAndOptimize()

bool holistic_fusion::GraphManager::addFactorsToRtSmootherAndOptimize	(	const gtsam::NonlinearFactorGraph &	newRtGraphFactors,
		const gtsam::Values &	newRtGraphValues,
		const std::map< gtsam::Key, double > &	newRtGraphKeysTimestampsMap,
		const std::shared_ptr< GraphConfig > &	graphConfigPtr,
		const int	additionalIterations )

private

Definition at line 1332 of file GraphManager.cpp.

◆ addFactorToRtAndBatchGraph_() [1/2]

template<class CHILDPTR>

bool holistic_fusion::GraphManager::addFactorToRtAndBatchGraph_	(	const gtsam::NoiseModelFactor *	noiseModelFactorPtr,
		const bool	addToOnlineSmootherFlag = true )

private

◆ addFactorToRtAndBatchGraph_() [2/2]

template<class CHILDPTR>

bool holistic_fusion::GraphManager::addFactorToRtAndBatchGraph_	(	const gtsam::NoiseModelFactor *	noiseModelFactorPtr,
		double	measurementTimestamp,
		const std::string &	measurementName,
		const bool	addToOnlineSmootherFlag = true )

private

◆ addImuFactorAndGetState()

void holistic_fusion::GraphManager::addImuFactorAndGetState	(	SafeIntegratedNavState &	returnPreIntegratedNavState,
		std::shared_ptr< SafeNavStateWithCovarianceAndBias > &	newOptimizedNavStatePtr,
		const std::shared_ptr< ImuBuffer > &	imuBufferPtr,
		double	imuTimeK,
		bool	createNewStateFlag )

Definition at line 229 of file GraphManager.cpp.

◆ addPoseBetweenFactor()

gtsam::Key holistic_fusion::GraphManager::addPoseBetweenFactor	(	const gtsam::Pose3 &	deltaPose,
		const Eigen::Matrix< double, 6, 1 > &	poseBetweenNoiseDensity,
		double	lidarTimeKm1,
		double	lidarTimeK,
		double	rate,
		const RobustNormEnum &	robustNormEnum,
		const double	robustNormConstant )

Definition at line 418 of file GraphManager.cpp.

◆ addUnaryClassicFactor()

template<class MEASUREMENT_TYPE, int NOISE_DIM, class FACTOR_TYPE, F SYMBOL_SHORTHAND, class... FACTOR_ARGS>

void holistic_fusion::GraphManager::addUnaryClassicFactor	(	const MEASUREMENT_TYPE &	unaryMeasurement,
		const Eigen::Matrix< double, NOISE_DIM, 1 > &	unaryNoiseDensity,
		double	measurementTime,
		const FACTOR_ARGS &...	factorArgs )

◆ addUnaryHolisticFactor()

template<class GMSF_EXPRESSION_TYPE>

void holistic_fusion::GraphManager::addUnaryHolisticFactor	(	const std::shared_ptr< GMSF_EXPRESSION_TYPE >	gmsfUnaryExpressionPtr,
		const bool	addToOnlineSmootherFlag = true )

◆ calculateNavStateAtGeneralKey()

gtsam::NavState holistic_fusion::GraphManager::calculateNavStateAtGeneralKey	(	bool &	computeSuccessfulFlag,
		std::shared_ptr< holistic_fusion::OptimizerBase >	graphPtr,
		const gtsam::Key &	generalKey,
		const char *	callingFunctionName )

staticprotected

Definition at line 482 of file GraphManager.cpp.

◆ calculatePoseCovarianceAtKeyInWorldFrame()

Eigen::Matrix< double, 6, 6 > holistic_fusion::GraphManager::calculatePoseCovarianceAtKeyInWorldFrame	(	std::shared_ptr< holistic_fusion::OptimizerBase >	graphPtr,
		const gtsam::Key &	graphKey,
		const char *	callingFunctionName,
		std::optional< const gtsam::NavState >	optionalNavState = std::nullopt )

staticprotected

Definition at line 504 of file GraphManager.cpp.

◆ calculateStateAtGeneralKey()

gtsam::NavState holistic_fusion::GraphManager::calculateStateAtGeneralKey	(	bool &	computeSuccessfulFlag,
		const gtsam::Key &	generalKey )

Definition at line 1250 of file GraphManager.cpp.

◆ extractBatchOptimizationResult_()

BatchOptimizationResult holistic_fusion::GraphManager::extractBatchOptimizationResult_ ( const bool computePoseCovarianceFlag )

private

Definition at line 1270 of file GraphManager.cpp.

◆ findGraphKeys_()

bool holistic_fusion::GraphManager::findGraphKeys_	(	gtsam::Key &	closestKeyKm1,
		gtsam::Key &	closestKeyK,
		double &	keyTimeStampDistance,
		double	maxTimestampDistance,
		double	timeKm1,
		double	timeK,
		const std::string &	name )

private

Find graph keys for timestamps.

Definition at line 1380 of file GraphManager.cpp.

◆ getInitAccBiasReference()

Eigen::Vector3d & holistic_fusion::GraphManager::getInitAccBiasReference ( )

inline

Getters.

Definition at line 132 of file GraphManager.h.

◆ getInitGyrBiasReference()

Eigen::Vector3d & holistic_fusion::GraphManager::getInitGyrBiasReference ( )

inline

Definition at line 133 of file GraphManager.h.

◆ getOptimizedGraphState()

const GraphState & holistic_fusion::GraphManager::getOptimizedGraphState ( )

inline

Definition at line 136 of file GraphManager.h.

◆ getPropagatedStateKey()

const gtsam::Key holistic_fusion::GraphManager::getPropagatedStateKey ( )

inline

Definition at line 137 of file GraphManager.h.

◆ getUnaryFactorGeneralKey()

bool holistic_fusion::GraphManager::getUnaryFactorGeneralKey	(	gtsam::Key &	returnedKey,
		double &	returnedGraphTime,
		const UnaryMeasurement &	unaryMeasurement )

Definition at line 380 of file GraphManager.cpp.

◆ hasReferenceFrameKeyframe()

bool holistic_fusion::GraphManager::hasReferenceFrameKeyframe	(	const std::string &	worldFrame,
		const std::string &	fixedFrame )

Definition at line 374 of file GraphManager.cpp.

◆ initImuIntegrators()

bool holistic_fusion::GraphManager::initImuIntegrators ( double gravityValue )

Position

Rotation

Earth rotation: angular velocity of the (gravity-aligned, z-up) world frame w.r.t. the inertial frame, expressed in the world frame. Setting it makes GTSAM (>= 4.3) use the exact rotating-frame model (Coriolis, centrifugal and Earth-rate terms). If left unset, the inertial model is used.

Bias

Definition at line 92 of file GraphManager.cpp.

◆ initPoseVelocityBiasGraph()

bool holistic_fusion::GraphManager::initPoseVelocityBiasGraph	(	double	timeStamp,
		const gtsam::Pose3 &	T_W_I0,
		const gtsam::Pose3 &	T_O_I0,
		const gtsam::Vector3 &	imuAngularVelocity )

Prior factor noise

Timestamp mapping for incremental fixed lag smoother

Add prior factor to graph and optimize for the first time -------------—

Definition at line 131 of file GraphManager.cpp.

◆ logLatencyAndUpdateDuration()

bool holistic_fusion::GraphManager::logLatencyAndUpdateDuration	(	const std::string &	savePath,
		const std::string &	timeString )

Definition at line 985 of file GraphManager.cpp.

◆ logRealTimeNavStates()

bool holistic_fusion::GraphManager::logRealTimeNavStates	(	const std::string &	savePath,
		const std::string &	timeString )

Definition at line 879 of file GraphManager.cpp.

◆ logRealTimeReferenceFrameStates()

bool holistic_fusion::GraphManager::logRealTimeReferenceFrameStates	(	const std::string &	savePath,
		const std::string &	timeString )

Definition at line 925 of file GraphManager.cpp.

◆ newPropagatedStateKey_()

const uint64_t holistic_fusion::GraphManager::newPropagatedStateKey_ ( )

inlineprivate

Generate new key.

Definition at line 178 of file GraphManager.h.

◆ optimizeSlowBatchSmoother() [1/2]

std::optional< BatchOptimizationResult > holistic_fusion::GraphManager::optimizeSlowBatchSmoother	(	int	maxIterations,
		const bool	computePoseCovarianceFlag )

Definition at line 871 of file GraphManager.cpp.

◆ optimizeSlowBatchSmoother() [2/2]

bool holistic_fusion::GraphManager::optimizeSlowBatchSmoother	(	int	maxIterations,
		const std::string &	savePath,
		const bool	saveCovarianceFlag )

Definition at line 862 of file GraphManager.cpp.

◆ runSlowBatchSmootherOptimization_()

bool holistic_fusion::GraphManager::runSlowBatchSmootherOptimization_ ( int maxIterations )

private

Definition at line 1256 of file GraphManager.cpp.

◆ saveOptimizedGraphToG2o()

void holistic_fusion::GraphManager::saveOptimizedGraphToG2o	(	const OptimizerBase &	optimizedGraph,
		const gtsam::Values &	optimizedValues,
		const std::string &	saveFileName )

static

Definition at line 1243 of file GraphManager.cpp.

◆ saveOptimizedValuesToFile()

void holistic_fusion::GraphManager::saveOptimizedValuesToFile	(	const gtsam::Values &	optimizedValues,
		const std::map< gtsam::Key, double > &	keyTimestampMap,
		const std::string &	savePath,
		const bool	saveCovarianceFlag )

Definition at line 1029 of file GraphManager.cpp.

◆ setInitialWorldFrameToFixedFrameTransform()

bool holistic_fusion::GraphManager::setInitialWorldFrameToFixedFrameTransform	(	const Eigen::Isometry3d &	T_W_F,
		const std::string &	fixedFrame )

Definition at line 364 of file GraphManager.cpp.

◆ updateGraph()

void holistic_fusion::GraphManager::updateGraph ( )

Definition at line 534 of file GraphManager.cpp.

◆ updateImuIntegrators_()

void holistic_fusion::GraphManager::updateImuIntegrators_ ( const TimeToImuMap & imuMeas )

private

Update IMU integrators.

Definition at line 1304 of file GraphManager.cpp.

◆ writeKeyToKeyTimeStampMap_()

void holistic_fusion::GraphManager::writeKeyToKeyTimeStampMap_	(	const gtsam::Key &	key,
		double	measurementTime,
		std::shared_ptr< std::map< gtsam::Key, double > >	keyTimestampMapPtr )

inlineprivate

Associate timestamp to each 'value key', e.g. for graph key 0, value keys (x0,v0,b0) need to be associated.

◆ writeValueKeysToKeyTimeStampMap_()

void holistic_fusion::GraphManager::writeValueKeysToKeyTimeStampMap_	(	const gtsam::Values &	values,
		double	measurementTime,
		std::shared_ptr< std::map< gtsam::Key, double > >	keyTimestampMapPtr )

private

Definition at line 1413 of file GraphManager.cpp.

Member Data Documentation

◆ batchFactorGraphBufferPtr_

std::shared_ptr<gtsam::NonlinearFactorGraph> holistic_fusion::GraphManager::batchFactorGraphBufferPtr_

private

Definition at line 216 of file GraphManager.h.

◆ batchGraphKeysTimestampsMapBufferPtr_

std::shared_ptr<std::map<gtsam::Key, double> > holistic_fusion::GraphManager::batchGraphKeysTimestampsMapBufferPtr_

private

Definition at line 222 of file GraphManager.h.

◆ batchGraphValuesBufferPtr_

std::shared_ptr<gtsam::Values> holistic_fusion::GraphManager::batchGraphValuesBufferPtr_

private

Definition at line 219 of file GraphManager.h.

◆ batchOptimizerPtr_

std::shared_ptr<OptimizerBase> holistic_fusion::GraphManager::batchOptimizerPtr_

private

Definition at line 213 of file GraphManager.h.

◆ fileLogger_

FileLogger holistic_fusion::GraphManager::fileLogger_

private

Definition at line 195 of file GraphManager.h.

◆ graphConfigPtr_

std::shared_ptr<holistic_fusion::GraphConfig> holistic_fusion::GraphManager::graphConfigPtr_ = nullptr

private

Config.

Definition at line 234 of file GraphManager.h.

◆ gtsamDynamicExpressionKeys_

DynamicDictionaryContainer holistic_fusion::GraphManager::gtsamDynamicExpressionKeys_

private

Definition at line 192 of file GraphManager.h.

◆ imuBiasPriorPtr_

std::shared_ptr<gtsam::imuBias::ConstantBias> holistic_fusion::GraphManager::imuBiasPriorPtr_

private

Definition at line 199 of file GraphManager.h.

◆ imuBufferPreintegratorPtr_

std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> holistic_fusion::GraphManager::imuBufferPreintegratorPtr_

private

Buffer Preintegrator.

Definition at line 228 of file GraphManager.h.

◆ imuFrame_

std::string holistic_fusion::GraphManager::imuFrame_

private

Definition at line 190 of file GraphManager.h.

◆ imuParamsPtr_

std::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> holistic_fusion::GraphManager::imuParamsPtr_

private

Definition at line 198 of file GraphManager.h.

◆ imuStepPreintegratorPtr_

std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> holistic_fusion::GraphManager::imuStepPreintegratorPtr_

private

Step Preintegrator.

Definition at line 226 of file GraphManager.h.

◆ lastImuVector_

gtsam::Vector6 holistic_fusion::GraphManager::lastImuVector_

private

IMU Buffer.

Definition at line 231 of file GraphManager.h.

◆ lastOptimizedStateTime_

double holistic_fusion::GraphManager::lastOptimizedStateTime_ = 0.0

private

Definition at line 208 of file GraphManager.h.

◆ latencyContainer_

std::map<double, double> holistic_fusion::GraphManager::latencyContainer_ = {}

private

Definition at line 246 of file GraphManager.h.

◆ latencyEndTime_

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::GraphManager::latencyEndTime_

private

Definition at line 244 of file GraphManager.h.

◆ latencyStartTime_

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::GraphManager::latencyStartTime_

private

Definition at line 243 of file GraphManager.h.

◆ latestGraphStateKeyAngularVelocity_

gtsam::Vector3 holistic_fusion::GraphManager::latestGraphStateKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0)

private

Definition at line 209 of file GraphManager.h.

◆ latestGraphStateKeyTime_

double holistic_fusion::GraphManager::latestGraphStateKeyTime_ = 0.0

private

Definition at line 207 of file GraphManager.h.

◆ O_imuPropagatedState_

gtsam::NavState holistic_fusion::GraphManager::O_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0))

private

Definition at line 203 of file GraphManager.h.

◆ operateOnGraphDataMutex_

std::mutex holistic_fusion::GraphManager::operateOnGraphDataMutex_

private

Mutex.

Definition at line 255 of file GraphManager.h.

◆ optimizationRunningMutex_

std::mutex holistic_fusion::GraphManager::optimizationRunningMutex_

private

Definition at line 256 of file GraphManager.h.

◆ optimizedGraphState_

holistic_fusion::GraphState holistic_fusion::GraphManager::optimizedGraphState_

private

Definition at line 200 of file GraphManager.h.

◆ propagatedImuSampleTime_

double holistic_fusion::GraphManager::propagatedImuSampleTime_ = 0.0

private

Definition at line 206 of file GraphManager.h.

◆ propagatedStateKey_

gtsam::Key holistic_fusion::GraphManager::propagatedStateKey_ = 0

private

Definition at line 205 of file GraphManager.h.

◆ realTimeOdomPoseContainer_

std::map<double, gtsam::Pose3> holistic_fusion::GraphManager::realTimeOdomPoseContainer_ = {}

private

Definition at line 238 of file GraphManager.h.

◆ realTimeReferenceFrameContainer_

std::map<double, TransformsDictionary<Eigen::Isometry3d> > holistic_fusion::GraphManager::realTimeReferenceFrameContainer_ = {}

private

Definition at line 240 of file GraphManager.h.

◆ realTimeReferenceFrameNamePairs_

std::set<std::pair<std::string, std::string> > holistic_fusion::GraphManager::realTimeReferenceFrameNamePairs_ = {}

private

Definition at line 241 of file GraphManager.h.

◆ realTimeWorldPoseContainer_

std::map<double, gtsam::Pose3> holistic_fusion::GraphManager::realTimeWorldPoseContainer_ = {}

private

Definition at line 237 of file GraphManager.h.

◆ rtFactorGraphBufferPtr_

std::shared_ptr<gtsam::NonlinearFactorGraph> holistic_fusion::GraphManager::rtFactorGraphBufferPtr_

private

Data buffer.

Definition at line 215 of file GraphManager.h.

◆ rtGraphKeysTimestampsMapBufferPtr_

std::shared_ptr<std::map<gtsam::Key, double> > holistic_fusion::GraphManager::rtGraphKeysTimestampsMapBufferPtr_

private

Definition at line 221 of file GraphManager.h.

◆ rtGraphValuesBufferPtr_

std::shared_ptr<gtsam::Values> holistic_fusion::GraphManager::rtGraphValuesBufferPtr_

private

Definition at line 218 of file GraphManager.h.

◆ rtOptimizerPtr_

std::shared_ptr<OptimizerBase> holistic_fusion::GraphManager::rtOptimizerPtr_

private

Definition at line 212 of file GraphManager.h.

◆ T_W_O_

Eigen::Isometry3d holistic_fusion::GraphManager::T_W_O_ = Eigen::Isometry3d::Identity()

private

Definition at line 204 of file GraphManager.h.

◆ timeToKeyBufferPtr_

std::shared_ptr<TimeGraphKeyBuffer> holistic_fusion::GraphManager::timeToKeyBufferPtr_

private

Definition at line 187 of file GraphManager.h.

◆ updateDurationContainer_

std::map<double, double> holistic_fusion::GraphManager::updateDurationContainer_ = {}

private

Definition at line 251 of file GraphManager.h.

◆ updateDurationEndTime_

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::GraphManager::updateDurationEndTime_

private

Definition at line 249 of file GraphManager.h.

◆ updateDurationStartTime_

std::chrono::time_point<std::chrono::high_resolution_clock> holistic_fusion::GraphManager::updateDurationStartTime_

private

Definition at line 248 of file GraphManager.h.

◆ W_imuPropagatedState_

gtsam::NavState holistic_fusion::GraphManager::W_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0))

private

Propagated state (at IMU frequency).

Definition at line 202 of file GraphManager.h.

◆ worldFrame_

std::string holistic_fusion::GraphManager::worldFrame_

private

Definition at line 191 of file GraphManager.h.

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

holistic_fusion/include/holistic_fusion/core/GraphManager.h
holistic_fusion/src/lib/GraphManager.cpp

Public Types

Public Member Functions

Static Public Member Functions

Static Protected Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ F

Constructor & Destructor Documentation

◆ GraphManager()

◆ ~GraphManager()

Member Function Documentation

◆ addFactorSafelyToRtAndBatchGraph_()

◆ addFactorsToBatchSmootherAndOptimize()

◆ addFactorsToRtSmootherAndOptimize()

◆ addFactorToRtAndBatchGraph_() [1/2]

◆ addFactorToRtAndBatchGraph_() [2/2]

◆ addImuFactorAndGetState()

◆ addPoseBetweenFactor()

◆ addUnaryClassicFactor()

◆ addUnaryHolisticFactor()

◆ calculateNavStateAtGeneralKey()

◆ calculatePoseCovarianceAtKeyInWorldFrame()

◆ calculateStateAtGeneralKey()

◆ extractBatchOptimizationResult_()

◆ findGraphKeys_()

◆ getInitAccBiasReference()

◆ getInitGyrBiasReference()

◆ getOptimizedGraphState()

◆ getPropagatedStateKey()

◆ getUnaryFactorGeneralKey()

◆ hasReferenceFrameKeyframe()

◆ initImuIntegrators()

◆ initPoseVelocityBiasGraph()

◆ logLatencyAndUpdateDuration()

◆ logRealTimeNavStates()

◆ logRealTimeReferenceFrameStates()

◆ newPropagatedStateKey_()

◆ optimizeSlowBatchSmoother() [1/2]

◆ optimizeSlowBatchSmoother() [2/2]

◆ runSlowBatchSmootherOptimization_()

◆ saveOptimizedGraphToG2o()

◆ saveOptimizedValuesToFile()

◆ setInitialWorldFrameToFixedFrameTransform()

◆ updateGraph()

◆ updateImuIntegrators_()

◆ writeKeyToKeyTimeStampMap_()

◆ writeValueKeysToKeyTimeStampMap_()

Member Data Documentation

◆ batchFactorGraphBufferPtr_

◆ batchGraphKeysTimestampsMapBufferPtr_

◆ batchGraphValuesBufferPtr_

◆ batchOptimizerPtr_

◆ fileLogger_

◆ graphConfigPtr_

◆ gtsamDynamicExpressionKeys_

◆ imuBiasPriorPtr_

◆ imuBufferPreintegratorPtr_

◆ imuFrame_

◆ imuParamsPtr_

◆ imuStepPreintegratorPtr_

◆ lastImuVector_

◆ lastOptimizedStateTime_

◆ latencyContainer_

◆ latencyEndTime_

◆ latencyStartTime_

◆ latestGraphStateKeyAngularVelocity_

◆ latestGraphStateKeyTime_

◆ O_imuPropagatedState_

◆ operateOnGraphDataMutex_

◆ optimizationRunningMutex_

◆ optimizedGraphState_

◆ propagatedImuSampleTime_

◆ propagatedStateKey_

◆ realTimeOdomPoseContainer_

◆ realTimeReferenceFrameContainer_

◆ realTimeReferenceFrameNamePairs_

◆ realTimeWorldPoseContainer_

◆ rtFactorGraphBufferPtr_