My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
holistic_fusion::HolisticFusion Class Referenceabstract

#include <HolisticFusion.h>

Inheritance diagram for holistic_fusion::HolisticFusion:

Public Member Functions

 HolisticFusion ()
 Constructor --------—.
virtual ~HolisticFusion ()
 Destructor --------—.
void setup (const std::shared_ptr< GraphConfig > graphConfigPtr, const std::shared_ptr< StaticTransforms > staticTransformsPtr)
bool initHeadingAndPosition (const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
bool initYawAndPosition (const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S1, const UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &M_t_M_S2)
bool initHeadingAndPositionAsIdentity ()
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_ ()
bool canInitYawAndPosition_ () const
 Initialization / Whether the yaw and the position can be initialized now. Prints the reason if not.
gtsam::Pose3 initialT_W_fixedFrame_ (const UnaryMeasurementAbsolute &measurement)
void setInitialOrientationAndPosition_ (const gtsam::Rot3 &R_W_I, const Eigen::Vector3d &W_t_W_S, const std::string &sensorFrame)
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 37 of file HolisticFusion.h.

Constructor & Destructor Documentation

◆ HolisticFusion()

holistic_fusion::HolisticFusion::HolisticFusion ( )

Constructor --------—.

Definition at line 24 of file HolisticFusion.cpp.

◆ ~HolisticFusion()

holistic_fusion::HolisticFusion::~HolisticFusion ( )
virtual

Destructor --------—.

Definition at line 29 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 204 of file HolisticFusion.cpp.

◆ addUnaryBearing3LandmarkMeasurement()

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

◆ addUnaryPitchAbsoluteMeasurement()

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

◆ 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

◆ 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

◆ addUnaryVelocity3AbsoluteMeasurement()

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

◆ addUnaryVelocity3LocalMeasurement()

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

◆ addUnaryYawAbsoluteMeasurement()

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

◆ addZeroMotionFactor()

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

Definition at line 350 of file HolisticFusion.cpp.

◆ addZeroVelocityFactor()

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

Definition at line 359 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 369 of file HolisticFusion.cpp.

◆ areRollAndPitchInited()

bool holistic_fusion::HolisticFusion::areRollAndPitchInited ( ) const

Definition at line 101 of file HolisticFusion.cpp.

◆ areYawAndPositionInited()

bool holistic_fusion::HolisticFusion::areYawAndPositionInited ( ) const

Definition at line 97 of file HolisticFusion.cpp.

◆ canInitYawAndPosition_()

bool holistic_fusion::HolisticFusion::canInitYawAndPosition_ ( ) const
private

Initialization / Whether the yaw and the position can be initialized now. Prints the reason if not.

Definition at line 163 of file HolisticFusion.cpp.

◆ checkAndPrintCovarianceViolation_()

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

Definition at line 481 of file HolisticFusion.cpp.

◆ getNormalOperationFlag()

bool holistic_fusion::HolisticFusion::getNormalOperationFlag ( ) const
inline

Definition at line 78 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 399 of file HolisticFusion.cpp.

◆ initHeadingAndPosition()

bool holistic_fusion::HolisticFusion::initHeadingAndPosition ( const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > & T_M_S)

Definition at line 110 of file HolisticFusion.cpp.

◆ initHeadingAndPositionAsIdentity()

bool holistic_fusion::HolisticFusion::initHeadingAndPositionAsIdentity ( )

Definition at line 152 of file HolisticFusion.cpp.

◆ initialT_W_fixedFrame_()

gtsam::Pose3 holistic_fusion::HolisticFusion::initialT_W_fixedFrame_ ( const UnaryMeasurementAbsolute & measurement)
private

Definition at line 175 of file HolisticFusion.cpp.

◆ initWorldFrameToFixedFrameTransform()

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

Definition at line 195 of file HolisticFusion.cpp.

◆ initYawAndPosition()

bool holistic_fusion::HolisticFusion::initYawAndPosition ( const UnaryMeasurementXDAbsolute< double, 1 > & yaw_W_S1,
const UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > & M_t_M_S2 )

Definition at line 129 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 115 of file HolisticFusion.h.

◆ isGraphInited()

bool holistic_fusion::HolisticFusion::isGraphInited ( ) const

Definition at line 105 of file HolisticFusion.cpp.

◆ logRealTimeNavStates()

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

Definition at line 84 of file HolisticFusion.cpp.

◆ optimizeGraph_()

void holistic_fusion::HolisticFusion::optimizeGraph_ ( )
private

Definition at line 426 of file HolisticFusion.cpp.

◆ optimizeSlowBatchSmoother() [1/2]

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

Definition at line 80 of file HolisticFusion.cpp.

◆ optimizeSlowBatchSmoother() [2/2]

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

Definition at line 76 of file HolisticFusion.cpp.

◆ pretendFirstMeasurementReceived()

void holistic_fusion::HolisticFusion::pretendFirstMeasurementReceived ( )
protected

Definition at line 476 of file HolisticFusion.cpp.

◆ setInitialOrientationAndPosition_()

void holistic_fusion::HolisticFusion::setInitialOrientationAndPosition_ ( const gtsam::Rot3 & R_W_I,
const Eigen::Vector3d & W_t_W_S,
const std::string & sensorFrame )
private

Definition at line 183 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 37 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 460 of file HolisticFusion.cpp.

Member Data Documentation

◆ alignedImuFlag_

bool holistic_fusion::HolisticFusion::alignedImuFlag_ = false
protected

Flags / Initialization.

Definition at line 148 of file HolisticFusion.h.

◆ coreImuBufferPtr_

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

Definition at line 141 of file HolisticFusion.h.

◆ foundInitialYawAndPositionFlag_

bool holistic_fusion::HolisticFusion::foundInitialYawAndPositionFlag_ = false
protected

Definition at line 149 of file HolisticFusion.h.

◆ graphConfigPtr_

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

Definition at line 134 of file HolisticFusion.h.

◆ graphMgrPtr_

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

Definition at line 139 of file HolisticFusion.h.

◆ imuCallbackCounter_

long holistic_fusion::HolisticFusion::imuCallbackCounter_ = 0
protected

Definition at line 157 of file HolisticFusion.h.

◆ initedGraphFlag_

bool holistic_fusion::HolisticFusion::initedGraphFlag_ = false
protected

Definition at line 150 of file HolisticFusion.h.

◆ initYawAndPositionMutex_

std::mutex holistic_fusion::HolisticFusion::initYawAndPositionMutex_
private

Definition at line 190 of file HolisticFusion.h.

◆ measurementsWithViolatedCovariance_

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

Definition at line 160 of file HolisticFusion.h.

◆ normalOperationFlag_

bool holistic_fusion::HolisticFusion::normalOperationFlag_ = false
protected

Definition at line 154 of file HolisticFusion.h.

◆ optimizeGraphFlag_

bool holistic_fusion::HolisticFusion::optimizeGraphFlag_ = false
protected

Definition at line 153 of file HolisticFusion.h.

◆ optimizeGraphMutex_

std::mutex holistic_fusion::HolisticFusion::optimizeGraphMutex_
protected

Definition at line 144 of file HolisticFusion.h.

◆ optimizeGraphThread_

std::thread holistic_fusion::HolisticFusion::optimizeGraphThread_
private

Definition at line 185 of file HolisticFusion.h.

◆ preIntegratedNavStatePtr_

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

State Containers.

Definition at line 193 of file HolisticFusion.h.

◆ staticTransformsPtr_

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

Definition at line 136 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 187 of file HolisticFusion.h.

◆ validFirstMeasurementReceivedFlag_

bool holistic_fusion::HolisticFusion::validFirstMeasurementReceivedFlag_ = false
protected

Definition at line 151 of file HolisticFusion.h.


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