#include <HolisticFusionDualGraph.h>

Inheritance diagram for holistic_fusion::HolisticFusionDualGraph:

Collaboration diagram for holistic_fusion::HolisticFusionDualGraph:

Public Member Functions
	HolisticFusionDualGraph ()
	~HolisticFusionDualGraph ()=default
virtual bool	setup ()
void	activateGlobalGraph (const gtsam::Vector3 &imuPosition, const gtsam::Rot3 &imuRotation, const double measurementTime)
void	activateFallbackGraph ()
std::shared_ptr< SafeNavState >	addDualOdometryMeasurementAndReturnNavState (const UnaryMeasurement6D &odometryKm1, const UnaryMeasurement6D &odometryK, const Eigen::Matrix< double, 6, 1 > &poseBetweenNoise)
	Return.
void	addDualGnssPositionMeasurement (const UnaryMeasurement3D &W_t_W_frame, const Eigen::Vector3d &lastPosition, const Eigen::Vector3d &gnssCovarianceXYZ, const bool attemptGraphSwitching, const bool addedYawBefore)
	No return.
bool	globalGraphActiveFlag ()
bool	fallbackGraphActiveFlag ()
void	activateFallbackGraph ()

Protected Member Functions
void	optimizeGraph_ ()
	Worker functions / Updating the factor graph.
bool	isGnssCovarianceViolated_ (const Eigen::Vector3d &gnssCovarianceXYZ)
Eigen::Vector3d	W_t_W_Frame1_to_W_t_W_Frame2_ (const Eigen::Vector3d &W_t_W_frame1, const std::string &frame1, const std::string &frame2, const Eigen::Matrix3d &R_W_frame2)
	Utility functions / Geometric transformation to IMU in world frame.

Private Attributes
std::shared_ptr< GraphManager >	graphMgrPtr_ = nullptr
std::vector< std::string >	graphNames_ {"globalGraph", "fallbackGraph"}
	Graph names.
std::shared_ptr< std::map< gtsam::Key, double > >	globalGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< std::map< gtsam::Key, double > >	fallbackGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< gtsam::IncrementalFixedLagSmoother >	activeSmootherPtr_ = globalSmootherPtr_
	Selector.
std::shared_ptr< gtsam::NonlinearFactorGraph >	activeFactorsBufferPtr_ = globalFactorsBufferPtr_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements >	activeImuBufferPreintegratorPtr_ = globalImuBufferPreintegratorPtr_
std::shared_ptr< gtsam::Values >	activeGraphValuesBufferPtr_ = globalGraphValuesBufferPtr_
std::shared_ptr< std::map< gtsam::Key, double > >	activeGraphKeysTimestampsMapBufferPtr_ = globalGraphKeysTimestampsMapBufferPtr_
std::mutex	activelyUsingActiveGraphMutex_
std::mutex	swappingActiveGraphMutex_
Private Attributes inherited from holistic_fusion::HolisticFusion
std::shared_ptr< GraphConfig >	graphConfigPtr_ = nullptr
std::shared_ptr< StaticTransforms >	staticTransformsPtr_ = nullptr
std::shared_ptr< GraphManager >	graphMgrPtr_ = nullptr
std::shared_ptr< holistic_fusion::ImuBuffer >	coreImuBufferPtr_
std::mutex	optimizeGraphMutex_
bool	alignedImuFlag_ = false
	Flags / Initialization.
bool	foundInitialYawAndPositionFlag_ = false
bool	initedGraphFlag_ = false
bool	validFirstMeasurementReceivedFlag_ = false
bool	optimizeGraphFlag_ = false
bool	normalOperationFlag_ = false
long	imuCallbackCounter_ = 0
std::unordered_set< std::string >	measurementsWithViolatedCovariance_

Additional Inherited Members
Private Member Functions inherited from holistic_fusion::HolisticFusion
	HolisticFusion ()
	Constructor --------—.
virtual	~HolisticFusion ()
	Destructor --------—.
void	setup (const std::shared_ptr< GraphConfig > graphConfigPtr, const std::shared_ptr< StaticTransforms > staticTransformsPtr)
bool	initYawAndPositionInWorld (const double yaw_W_frame1, const Eigen::Vector3d &W_t_W_frame2, const std::string &frame1, const std::string &frame2)
bool	initYawAndPosition (const UnaryMeasurementXD< Eigen::Isometry3d, 6 > &unary6DMeasurement)
bool	initWorldFrameToFixedFrameTransform (const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
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)
bool	areYawAndPositionInited () const
bool	areRollAndPitchInited () const
bool	isGraphInited () const
bool	getNormalOperationFlag () const
bool	addCoreImuMeasurementAndGetState (const Eigen::Vector3d &linearAcc, const Eigen::Vector3d &angularVel, const double imuTimeK, std::shared_ptr< SafeIntegratedNavState > &returnPreIntegratedNavStatePtr, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &returnOptimizedStateWithCovarianceAndBiasPtr, Eigen::Matrix< double, 6, 1 > &returnAddedImuMeasurements)
	Main: IMU --------------------—.
virtual void	addUnaryPose3AbsoluteMeasurement (const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &R_T_R_S, const bool addToOnlineSmootherFlag=true)=0
	Unary Measurements / Absolute Measurements.
virtual void	addUnaryPosition3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_t_R_S)=0
virtual void	addUnaryVelocity3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_v_R_S)=0
virtual void	addUnaryRollAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &roll_R_S)=0
virtual void	addUnaryPitchAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &pitch_R_S)=0
virtual void	addUnaryYawAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &yaw_R_S)=0
virtual void	addUnaryVelocity3LocalMeasurement (UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_R_S)=0
virtual void	addUnaryPosition3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_t_S_L, const int landmarkCreationCounter)=0
	Landmark Measurements.
virtual void	addUnaryBearing3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_bearing_S_L)=0
virtual void	addBinaryPose3Measurement (const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &delta)=0
	Binary Measurements.
bool	addZeroMotionFactor (double timeKm1, double timeK, double noiseDensity)
bool	addZeroVelocityFactor (double timeK, double noiseDensity)
template<int DIM>
bool	isCovarianceViolated_ (const Eigen::Matrix< double, DIM, 1 > &covariance, const double covarianceViolationThreshold)
	Convenience functions.
void	pretendFirstMeasurementReceived ()
bool	checkAndPrintCovarianceViolation_ (const std::string &measurementName, const bool violatedFlag)

Detailed Description

Definition at line 26 of file HolisticFusionDualGraph.h.

Constructor & Destructor Documentation

◆ HolisticFusionDualGraph()

holistic_fusion::HolisticFusionDualGraph::HolisticFusionDualGraph ( )

◆ ~HolisticFusionDualGraph()

holistic_fusion::HolisticFusionDualGraph::~HolisticFusionDualGraph ( )

default

Member Function Documentation

◆ activateFallbackGraph() [1/2]

void holistic_fusion::HolisticFusionDualGraph::activateFallbackGraph ( )

◆ activateFallbackGraph() [2/2]

void holistic_fusion::HolisticFusionDualGraph::activateFallbackGraph ( )

◆ activateGlobalGraph()

void holistic_fusion::HolisticFusionDualGraph::activateGlobalGraph	(	const gtsam::Vector3 &	imuPosition,
		const gtsam::Rot3 &	imuRotation,
		const double	measurementTime )

◆ addDualGnssPositionMeasurement()

void holistic_fusion::HolisticFusionDualGraph::addDualGnssPositionMeasurement	(	const UnaryMeasurement3D &	W_t_W_frame,
		const Eigen::Vector3d &	lastPosition,
		const Eigen::Vector3d &	gnssCovarianceXYZ,
		const bool	attemptGraphSwitching,
		const bool	addedYawBefore )

No return.

◆ addDualOdometryMeasurementAndReturnNavState()

std::shared_ptr< SafeNavState > holistic_fusion::HolisticFusionDualGraph::addDualOdometryMeasurementAndReturnNavState	(	const UnaryMeasurement6D &	odometryKm1,
		const UnaryMeasurement6D &	odometryK,
		const Eigen::Matrix< double, 6, 1 > &	poseBetweenNoise )

Return.

◆ fallbackGraphActiveFlag()

bool holistic_fusion::HolisticFusionDualGraph::fallbackGraphActiveFlag ( )

inline

Definition at line 54 of file HolisticFusionDualGraph.h.

◆ globalGraphActiveFlag()

bool holistic_fusion::HolisticFusionDualGraph::globalGraphActiveFlag ( )

inline

Definition at line 50 of file HolisticFusionDualGraph.h.

◆ isGnssCovarianceViolated_()

bool holistic_fusion::HolisticFusionDualGraph::isGnssCovarianceViolated_ ( const Eigen::Vector3d & gnssCovarianceXYZ )

protected

◆ optimizeGraph_()

void holistic_fusion::HolisticFusionDualGraph::optimizeGraph_ ( )

protected

Worker functions / Updating the factor graph.

◆ setup()

virtual bool holistic_fusion::HolisticFusionDualGraph::setup ( )

virtual

◆ W_t_W_Frame1_to_W_t_W_Frame2_()

Eigen::Vector3d holistic_fusion::HolisticFusionDualGraph::W_t_W_Frame1_to_W_t_W_Frame2_	(	const Eigen::Vector3d &	W_t_W_frame1,
		const std::string &	frame1,
		const std::string &	frame2,
		const Eigen::Matrix3d &	R_W_frame2 )

protected

Utility functions / Geometric transformation to IMU in world frame.

Member Data Documentation

◆ activeFactorsBufferPtr_

std::shared_ptr<gtsam::NonlinearFactorGraph> holistic_fusion::HolisticFusionDualGraph::activeFactorsBufferPtr_ = globalFactorsBufferPtr_

private

Definition at line 88 of file HolisticFusionDualGraph.h.

◆ activeGraphKeysTimestampsMapBufferPtr_

std::shared_ptr<std::map<gtsam::Key, double> > holistic_fusion::HolisticFusionDualGraph::activeGraphKeysTimestampsMapBufferPtr_ = globalGraphKeysTimestampsMapBufferPtr_

private

Definition at line 91 of file HolisticFusionDualGraph.h.

◆ activeGraphValuesBufferPtr_

std::shared_ptr<gtsam::Values> holistic_fusion::HolisticFusionDualGraph::activeGraphValuesBufferPtr_ = globalGraphValuesBufferPtr_

private

Definition at line 90 of file HolisticFusionDualGraph.h.

◆ activeImuBufferPreintegratorPtr_

std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> holistic_fusion::HolisticFusionDualGraph::activeImuBufferPreintegratorPtr_ = globalImuBufferPreintegratorPtr_

private

Definition at line 89 of file HolisticFusionDualGraph.h.

◆ activelyUsingActiveGraphMutex_

std::mutex holistic_fusion::HolisticFusionDualGraph::activelyUsingActiveGraphMutex_

private

Definition at line 94 of file HolisticFusionDualGraph.h.

◆ activeSmootherPtr_

std::shared_ptr<gtsam::IncrementalFixedLagSmoother> holistic_fusion::HolisticFusionDualGraph::activeSmootherPtr_ = globalSmootherPtr_

private

Selector.

Definition at line 87 of file HolisticFusionDualGraph.h.

◆ fallbackGraphKeysTimestampsMapBufferPtr_

std::shared_ptr<std::map<gtsam::Key, double> > holistic_fusion::HolisticFusionDualGraph::fallbackGraphKeysTimestampsMapBufferPtr_

private

Definition at line 84 of file HolisticFusionDualGraph.h.

◆ globalGraphKeysTimestampsMapBufferPtr_

std::shared_ptr<std::map<gtsam::Key, double> > holistic_fusion::HolisticFusionDualGraph::globalGraphKeysTimestampsMapBufferPtr_

private

Definition at line 83 of file HolisticFusionDualGraph.h.

◆ graphMgrPtr_

std::shared_ptr<GraphManager> holistic_fusion::HolisticFusionDualGraph::graphMgrPtr_ = nullptr

private

Definition at line 77 of file HolisticFusionDualGraph.h.

◆ graphNames_

std::vector<std::string> holistic_fusion::HolisticFusionDualGraph::graphNames_ {"globalGraph", "fallbackGraph"}

private

Graph names.

Definition at line 80 of file HolisticFusionDualGraph.h.

◆ swappingActiveGraphMutex_

std::mutex holistic_fusion::HolisticFusionDualGraph::swappingActiveGraphMutex_

private

Definition at line 95 of file HolisticFusionDualGraph.h.

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

holistic_fusion/include/holistic_fusion/interface/HolisticFusionDualGraph.h

Public Member Functions

Protected Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ HolisticFusionDualGraph()

◆ ~HolisticFusionDualGraph()

Member Function Documentation

◆ activateFallbackGraph() [1/2]

◆ activateFallbackGraph() [2/2]

◆ activateGlobalGraph()

◆ addDualGnssPositionMeasurement()

◆ addDualOdometryMeasurementAndReturnNavState()

◆ fallbackGraphActiveFlag()

◆ globalGraphActiveFlag()

◆ isGnssCovarianceViolated_()

◆ optimizeGraph_()

◆ setup()

◆ W_t_W_Frame1_to_W_t_W_Frame2_()

Member Data Documentation

◆ activeFactorsBufferPtr_

◆ activeGraphKeysTimestampsMapBufferPtr_

◆ activeGraphValuesBufferPtr_

◆ activeImuBufferPreintegratorPtr_

◆ activelyUsingActiveGraphMutex_

◆ activeSmootherPtr_

◆ fallbackGraphKeysTimestampsMapBufferPtr_

◆ globalGraphKeysTimestampsMapBufferPtr_

◆ graphMgrPtr_

◆ graphNames_

◆ swappingActiveGraphMutex_