#include <HolisticFusion.h>

Inheritance diagram for holistic_fusion::HolisticFusion:

[legend]

Public Member Functions
	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)

Protected Member Functions
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)

Protected Attributes
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_

Private Member Functions
bool	alignImu_ (double &imuAttitudeRoll, double &imuAttitudePitch)
	Worker functions / Set Imu Attitude.
void	initGraph_ (const double timeStamp_k, const Eigen::Vector3d &imuAngularVelocity)
void	optimizeGraph_ ()
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::thread	optimizeGraphThread_
std::atomic< bool >	stopOptimizeGraphThread_ {false}
std::mutex	initYawAndPositionMutex_
std::shared_ptr< SafeIntegratedNavState >	preIntegratedNavStatePtr_ = nullptr
	State Containers.

Detailed Description

Definition at line 34 of file HolisticFusion.h.

Constructor & Destructor Documentation

◆ HolisticFusion()

holistic_fusion::HolisticFusion::HolisticFusion ( )

Constructor --------—.

Definition at line 23 of file HolisticFusion.cpp.

◆ ~HolisticFusion()

holistic_fusion::HolisticFusion::~HolisticFusion ( )

virtual

Destructor --------—.

Definition at line 28 of file HolisticFusion.cpp.

Member Function Documentation

◆ addBinaryPose3Measurement()

virtual void holistic_fusion::HolisticFusion::addBinaryPose3Measurement ( const BinaryMeasurementXD< Eigen::Isometry3d, 6 > & delta )

pure virtual

Binary Measurements.

Implemented in holistic_fusion::HolisticFusionClassic.

◆ addCoreImuMeasurementAndGetState()

bool holistic_fusion::HolisticFusion::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 --------------------—.

Definition at line 182 of file HolisticFusion.cpp.

◆ addUnaryBearing3LandmarkMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryBearing3LandmarkMeasurement ( UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > & S_bearing_S_L )

pure virtual

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryPitchAbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryPitchAbsoluteMeasurement ( const UnaryMeasurementXDAbsolute< double, 1 > & pitch_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionClassic.

◆ addUnaryPose3AbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryPose3AbsoluteMeasurement	(	const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &	R_T_R_S,
		const bool	addToOnlineSmootherFlag = true )

pure virtual

Unary Measurements / Absolute Measurements.

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryPosition3AbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryPosition3AbsoluteMeasurement ( UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > & R_t_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryPosition3LandmarkMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryPosition3LandmarkMeasurement	(	UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &	S_t_S_L,
		const int	landmarkCreationCounter )

pure virtual

Landmark Measurements.

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryRollAbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryRollAbsoluteMeasurement ( const UnaryMeasurementXDAbsolute< double, 1 > & roll_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionClassic.

◆ addUnaryVelocity3AbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryVelocity3AbsoluteMeasurement ( UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > & R_v_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryVelocity3LocalMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryVelocity3LocalMeasurement ( UnaryMeasurementXD< Eigen::Vector3d, 3 > & S_v_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionHolistic.

◆ addUnaryYawAbsoluteMeasurement()

virtual void holistic_fusion::HolisticFusion::addUnaryYawAbsoluteMeasurement ( const UnaryMeasurementXDAbsolute< double, 1 > & yaw_R_S )

pure virtual

Implemented in holistic_fusion::HolisticFusionClassic.

◆ addZeroMotionFactor()

bool holistic_fusion::HolisticFusion::addZeroMotionFactor	(	double	timeKm1,
		double	timeK,
		double	noiseDensity )

Definition at line 328 of file HolisticFusion.cpp.

◆ addZeroVelocityFactor()

bool holistic_fusion::HolisticFusion::addZeroVelocityFactor	(	double	timeK,
		double	noiseDensity )

Definition at line 337 of file HolisticFusion.cpp.

◆ alignImu_()

bool holistic_fusion::HolisticFusion::alignImu_	(	double &	imuAttitudeRoll,
		double &	imuAttitudePitch )

private

Worker functions / Set Imu Attitude.

Worker Functions --------------------—.

Definition at line 347 of file HolisticFusion.cpp.

◆ areRollAndPitchInited()

bool holistic_fusion::HolisticFusion::areRollAndPitchInited ( ) const

Definition at line 100 of file HolisticFusion.cpp.

◆ areYawAndPositionInited()

bool holistic_fusion::HolisticFusion::areYawAndPositionInited ( ) const

Definition at line 96 of file HolisticFusion.cpp.

◆ checkAndPrintCovarianceViolation_()

bool holistic_fusion::HolisticFusion::checkAndPrintCovarianceViolation_	(	const std::string &	measurementName,
		const bool	violatedFlag )

protected

Definition at line 459 of file HolisticFusion.cpp.

◆ getNormalOperationFlag()

bool holistic_fusion::HolisticFusion::getNormalOperationFlag ( ) const

inline

Definition at line 64 of file HolisticFusion.h.

◆ initGraph_()

void holistic_fusion::HolisticFusion::initGraph_	(	const double	timeStamp_k,
		const Eigen::Vector3d &	imuAngularVelocity )

private

Initialize graph node

Definition at line 377 of file HolisticFusion.cpp.

◆ initWorldFrameToFixedFrameTransform()

bool holistic_fusion::HolisticFusion::initWorldFrameToFixedFrameTransform	(	const Eigen::Isometry3d &	T_W_F,
		const std::string &	fixedFrame )

Definition at line 173 of file HolisticFusion.cpp.

◆ initYawAndPosition()

bool holistic_fusion::HolisticFusion::initYawAndPosition ( const UnaryMeasurementXD< Eigen::Isometry3d, 6 > & unary6DMeasurement )

Definition at line 167 of file HolisticFusion.cpp.

◆ initYawAndPositionInWorld()

bool holistic_fusion::HolisticFusion::initYawAndPositionInWorld	(	const double	yaw_W_frame1,
		const Eigen::Vector3d &	W_t_W_frame2,
		const std::string &	frame1,
		const std::string &	frame2 )

Definition at line 109 of file HolisticFusion.cpp.

◆ isCovarianceViolated_()

template<int DIM>

bool holistic_fusion::HolisticFusion::isCovarianceViolated_	(	const Eigen::Matrix< double, DIM, 1 > &	covariance,
		const double	covarianceViolationThreshold )

inlineprotected

Convenience functions.

Definition at line 101 of file HolisticFusion.h.

◆ isGraphInited()

bool holistic_fusion::HolisticFusion::isGraphInited ( ) const

Definition at line 104 of file HolisticFusion.cpp.

◆ logRealTimeNavStates()

bool holistic_fusion::HolisticFusion::logRealTimeNavStates ( const std::string & savePath )

Definition at line 83 of file HolisticFusion.cpp.

◆ optimizeGraph_()

void holistic_fusion::HolisticFusion::optimizeGraph_ ( )

private

Definition at line 404 of file HolisticFusion.cpp.

◆ optimizeSlowBatchSmoother() [1/2]

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

Definition at line 79 of file HolisticFusion.cpp.

◆ optimizeSlowBatchSmoother() [2/2]

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

Definition at line 75 of file HolisticFusion.cpp.

◆ pretendFirstMeasurementReceived()

void holistic_fusion::HolisticFusion::pretendFirstMeasurementReceived ( )

protected

Definition at line 454 of file HolisticFusion.cpp.

◆ setup()

void holistic_fusion::HolisticFusion::setup	(	const std::shared_ptr< GraphConfig >	graphConfigPtr,
		const std::shared_ptr< StaticTransforms >	staticTransformsPtr )

Initialize helper threads

Definition at line 36 of file HolisticFusion.cpp.

◆ W_t_W_Frame1_to_W_t_W_Frame2_()

Eigen::Vector3d holistic_fusion::HolisticFusion::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 )

private

Utility functions / Geometric transformation to IMU in world frame.

Utility Functions --------------------—.

Global rotation

Translation in global frame

Shift observed Gnss position to IMU frame (instead of Gnss antenna)

Definition at line 438 of file HolisticFusion.cpp.

Member Data Documentation

◆ alignedImuFlag_

bool holistic_fusion::HolisticFusion::alignedImuFlag_ = false

protected

Flags / Initialization.

Definition at line 134 of file HolisticFusion.h.

◆ coreImuBufferPtr_

std::shared_ptr<holistic_fusion::ImuBuffer> holistic_fusion::HolisticFusion::coreImuBufferPtr_

protected

Definition at line 127 of file HolisticFusion.h.

◆ foundInitialYawAndPositionFlag_

bool holistic_fusion::HolisticFusion::foundInitialYawAndPositionFlag_ = false

protected

Definition at line 135 of file HolisticFusion.h.

◆ graphConfigPtr_

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

protected

Definition at line 120 of file HolisticFusion.h.

◆ graphMgrPtr_

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

protected

Definition at line 125 of file HolisticFusion.h.

◆ imuCallbackCounter_

long holistic_fusion::HolisticFusion::imuCallbackCounter_ = 0

protected

Definition at line 143 of file HolisticFusion.h.

◆ initedGraphFlag_

bool holistic_fusion::HolisticFusion::initedGraphFlag_ = false

protected

Definition at line 136 of file HolisticFusion.h.

◆ initYawAndPositionMutex_

std::mutex holistic_fusion::HolisticFusion::initYawAndPositionMutex_

private

Definition at line 168 of file HolisticFusion.h.

◆ measurementsWithViolatedCovariance_

std::unordered_set<std::string> holistic_fusion::HolisticFusion::measurementsWithViolatedCovariance_

protected

Definition at line 146 of file HolisticFusion.h.

◆ normalOperationFlag_

bool holistic_fusion::HolisticFusion::normalOperationFlag_ = false

protected

Definition at line 140 of file HolisticFusion.h.

◆ optimizeGraphFlag_

bool holistic_fusion::HolisticFusion::optimizeGraphFlag_ = false

protected

Definition at line 139 of file HolisticFusion.h.

◆ optimizeGraphMutex_

std::mutex holistic_fusion::HolisticFusion::optimizeGraphMutex_

protected

Definition at line 130 of file HolisticFusion.h.

◆ optimizeGraphThread_

std::thread holistic_fusion::HolisticFusion::optimizeGraphThread_

private

Definition at line 163 of file HolisticFusion.h.

◆ preIntegratedNavStatePtr_

std::shared_ptr<SafeIntegratedNavState> holistic_fusion::HolisticFusion::preIntegratedNavStatePtr_ = nullptr

private

State Containers.

Definition at line 171 of file HolisticFusion.h.

◆ staticTransformsPtr_

std::shared_ptr<StaticTransforms> holistic_fusion::HolisticFusion::staticTransformsPtr_ = nullptr

protected

Definition at line 122 of file HolisticFusion.h.

◆ stopOptimizeGraphThread_

std::atomic<bool> holistic_fusion::HolisticFusion::stopOptimizeGraphThread_ {false}

private

Thread 5: Update of the graph as soon as new lidar measurement has arrived

Definition at line 165 of file HolisticFusion.h.

◆ validFirstMeasurementReceivedFlag_

bool holistic_fusion::HolisticFusion::validFirstMeasurementReceivedFlag_ = false

protected

Definition at line 137 of file HolisticFusion.h.

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

holistic_fusion/include/holistic_fusion/interface/HolisticFusion.h
holistic_fusion/src/lib/HolisticFusion.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ HolisticFusion()

◆ ~HolisticFusion()

Member Function Documentation

◆ addBinaryPose3Measurement()

◆ addCoreImuMeasurementAndGetState()

◆ addUnaryBearing3LandmarkMeasurement()

◆ addUnaryPitchAbsoluteMeasurement()

◆ addUnaryPose3AbsoluteMeasurement()

◆ addUnaryPosition3AbsoluteMeasurement()

◆ addUnaryPosition3LandmarkMeasurement()

◆ addUnaryRollAbsoluteMeasurement()

◆ addUnaryVelocity3AbsoluteMeasurement()

◆ addUnaryVelocity3LocalMeasurement()

◆ addUnaryYawAbsoluteMeasurement()

◆ addZeroMotionFactor()

◆ addZeroVelocityFactor()

◆ alignImu_()

◆ areRollAndPitchInited()

◆ areYawAndPositionInited()

◆ checkAndPrintCovarianceViolation_()

◆ getNormalOperationFlag()

◆ initGraph_()

◆ initWorldFrameToFixedFrameTransform()

◆ initYawAndPosition()

◆ initYawAndPositionInWorld()

◆ isCovarianceViolated_()

◆ isGraphInited()

◆ logRealTimeNavStates()

◆ optimizeGraph_()

◆ optimizeSlowBatchSmoother() [1/2]

◆ optimizeSlowBatchSmoother() [2/2]

◆ pretendFirstMeasurementReceived()

◆ setup()

◆ W_t_W_Frame1_to_W_t_W_Frame2_()

Member Data Documentation

◆ alignedImuFlag_

◆ coreImuBufferPtr_

◆ foundInitialYawAndPositionFlag_

◆ graphConfigPtr_

◆ graphMgrPtr_

◆ imuCallbackCounter_

◆ initedGraphFlag_

◆ initYawAndPositionMutex_

◆ measurementsWithViolatedCovariance_

◆ normalOperationFlag_

◆ optimizeGraphFlag_

◆ optimizeGraphMutex_

◆ optimizeGraphThread_

◆ preIntegratedNavStatePtr_

◆ staticTransformsPtr_

◆ stopOptimizeGraphThread_

◆ validFirstMeasurementReceivedFlag_