#include <ImuPose3Fuser.h>

Inheritance diagram for imu_pose3_fuser::ImuPose3Fuser:

[legend]

Collaboration diagram for imu_pose3_fuser::ImuPose3Fuser:

[legend]

Public Member Functions
	ImuPose3Fuser (std::shared_ptr< ros::NodeHandle > privateNodePtr)
	~ImuPose3Fuser ()=default
void	setup ()
Public Member Functions inherited from holistic_fusion::HolisticFusionRos
	HolisticFusionRos (const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
	~HolisticFusionRos () override=default
void	setup (const std::shared_ptr< StaticTransforms > staticTransformsPtr)
Public Member Functions inherited from holistic_fusion::HolisticFusionClassic
	HolisticFusionClassic ()
virtual	~HolisticFusionClassic ()=default
void	addUnaryRollAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &roll_F_S) override
	Unary Measurements / Absolute Measurements.
void	addUnaryPitchAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &pitch_F_S) override
void	addUnaryYawAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S) override
void	addBinaryPose3Measurement (const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &F_T_F_S) override
	Binary Measurements.
Public 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 --------------------—.
bool	addZeroMotionFactor (double timeKm1, double timeK, double noiseDensity)
bool	addZeroVelocityFactor (double timeK, double noiseDensity)
Public Member Functions inherited from holistic_fusion::HolisticFusionHolistic
	HolisticFusionHolistic ()
virtual	~HolisticFusionHolistic ()=default
void	addUnaryPose3AbsoluteMeasurement (const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &R_T_R_S, const bool addToOnlineSmootherFlag=true) override
	Unary Measurements / Absolute Measurements: In reference frame --> systematic drift.
void	addUnaryPosition3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_t_R_S) override
void	addUnaryHeadingAbsoluteMeasurement (const UnaryMeasurementXDAbsolute< Eigen::Matrix3d, 1 > &R_M_S)
void	addUnaryVelocity3AbsoluteMeasurement (UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_v_R_S) override
void	addUnaryVelocity3LocalMeasurement (UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S) override
void	addUnaryVelocity3LocalMovingFrameMeasurement (UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S, const Eigen::Isometry3d &T_I_sensorFrame, const Eigen::Vector3d &I_w_W_I)
void	addUnaryPosition3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_t_S_L, const int landmarkCreationCounter) override
	Landmark Measurements: No systematic drift.
void	addUnaryBearing3LandmarkMeasurement (UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_bearing_S_L) override

Protected Member Functions
void	initializePublishers (ros::NodeHandle &privateNode) override
void	initializeSubscribers (ros::NodeHandle &privateNode) override
void	initializeMessages (ros::NodeHandle &privateNode) override
void	initializeServices (ros::NodeHandle &privateNode) override
void	readParams (const ros::NodeHandle &privateNode) override
Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
virtual void	imuCallback (const sensor_msgs::Imu::ConstPtr &imuPtr)
virtual bool	srvOfflineSmootherOptimizeCallback (holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res)
bool	srvLogRealTimeNavStatesCallback (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
virtual void	publishState (const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > &integratedNavStatePtr, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr)
void	publishNonTimeCriticalData (const Eigen::Matrix< double, 6, 6 > poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > twistCovarianceRos, const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > integratedNavStatePtr, const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr)
void	publishOptimizedStateAndBias (const std::shared_ptr< const holistic_fusion::SafeNavStateWithCovarianceAndBias > optimizedStateWithCovarianceAndBiasPtr, const Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > &twistCovarianceRos)
void	publishTfTreeTransform (const std::string &frameName, const std::string &childFrameName, double timeStamp, const Eigen::Isometry3d &T_frame_childFrame)
void	publishImuOdoms (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &preIntegratedNavStatePtr, const Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, const Eigen::Matrix< double, 6, 6 > &twistCovarianceRos) const
void	publishDiagVarianceVectors (const Eigen::Vector3d &posVarianceRos, const Eigen::Vector3d &rotVarianceRos, const double timeStamp) const
void	publishVelocityMarkers (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &navStatePtr) const
void	publishImuPaths (const std::shared_ptr< const holistic_fusion::SafeIntegratedNavState > &navStatePtr) const
void	publishAddedImuMeas (const Eigen::Matrix< double, 6, 1 > &addedImuMeas, const ros::Time &stamp) const
long	secondsSinceStart ()
Protected Member Functions inherited from holistic_fusion::HolisticFusion
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)

Private Member Functions
void	pose3Callback_ (const nav_msgs::Odometry::ConstPtr &lidarOdomPtr)

Private Attributes
std::chrono::time_point< std::chrono::high_resolution_clock >	startTime_
std::chrono::time_point< std::chrono::high_resolution_clock >	currentTime_
double	pose3OdometryRate_ = 5.0
Eigen::Matrix< double, 6, 1 >	pose3UnaryNoise_
ros::Subscriber	subPose3Odometry_
ros::Publisher	pubMeasPose3Path_
nav_msgs::PathPtr	measPose3_worldImuPathPtr_
std::string	unaryPose3Frame_

Additional Inherited Members
Static Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
static void	addToPathMsg (const nav_msgs::PathPtr &pathPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Vector3d &t, int maxBufferLength)
static void	addToOdometryMsg (const nav_msgs::OdometryPtr &msgPtr, const std::string &fixedFrame, const std::string &movingFrame, const ros::Time &stamp, const Eigen::Isometry3d &T, const Eigen::Vector3d &W_v_W_F, const Eigen::Vector3d &W_w_W_F, const Eigen::Matrix< double, 6, 6 > &poseCovariance=Eigen::Matrix< double, 6, 6 >::Zero(), const Eigen::Matrix< double, 6, 6 > &twistCovariance=Eigen::Matrix< double, 6, 6 >::Zero())
static void	addToPoseWithCovarianceStampedMsg (const geometry_msgs::PoseWithCovarianceStampedPtr &msgPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Isometry3d &T, const Eigen::Matrix< double, 6, 6 > &transformCovariance=Eigen::Matrix< double, 6, 6 >::Zero())
static void	addToLandmarkMarkerArrayMsg (const visualization_msgs::MarkerArrayPtr &markerArrayPtr, const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Isometry3d &T_R_L, const std::string &transformName)
static void	extractCovariancesFromOptimizedState (Eigen::Matrix< double, 6, 6 > &poseCovarianceRos, Eigen::Matrix< double, 6, 6 > &twistCovarianceRos, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr)
static void	createVelocityMarker (const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Vector3d &velocity, const Eigen::Vector3d &colorRgb, visualization_msgs::Marker &marker)
static void	createContactMarker (const std::string &referenceFrameName, const ros::Time &stamp, const Eigen::Vector3d &position, const std::string &nameSpace, const int id, const Eigen::Vector3d &colorRgb, visualization_msgs::Marker &marker)
Protected Attributes inherited from holistic_fusion::HolisticFusionRos
ros::NodeHandle	privateNode
std::chrono::time_point< std::chrono::high_resolution_clock >	startTime
std::chrono::time_point< std::chrono::high_resolution_clock >	currentTime
tf::TransformBroadcaster	tfBroadcaster
std::string	referenceFrameAlignedNameId = "_holistic_fusion_aligned"
std::string	sensorFrameCorrectedNameId = "_holistic_fusion_corrected"
std::string	optimizationResultLoggingPath = ""
Protected 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_

Detailed Description

Definition at line 29 of file ImuPose3Fuser.h.

Constructor & Destructor Documentation

◆ ImuPose3Fuser()

imu_pose3_fuser::ImuPose3Fuser::ImuPose3Fuser ( std::shared_ptr< ros::NodeHandle > privateNodePtr )

Definition at line 20 of file ImuPose3Fuser.cpp.

◆ ~ImuPose3Fuser()

imu_pose3_fuser::ImuPose3Fuser::~ImuPose3Fuser ( )

default

Member Function Documentation

◆ initializeMessages()

void imu_pose3_fuser::ImuPose3Fuser::initializeMessages ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 72 of file ImuPose3Fuser.cpp.

◆ initializePublishers()

void imu_pose3_fuser::ImuPose3Fuser::initializePublishers ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 60 of file ImuPose3Fuser.cpp.

◆ initializeServices()

void imu_pose3_fuser::ImuPose3Fuser::initializeServices ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 77 of file ImuPose3Fuser.cpp.

◆ initializeSubscribers()

void imu_pose3_fuser::ImuPose3Fuser::initializeSubscribers ( ros::NodeHandle & privateNode )

overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 65 of file ImuPose3Fuser.cpp.

◆ pose3Callback_()

void imu_pose3_fuser::ImuPose3Fuser::pose3Callback_ ( const nav_msgs::Odometry::ConstPtr & lidarOdomPtr )

private

Definition at line 81 of file ImuPose3Fuser.cpp.

◆ readParams()

void imu_pose3_fuser::ImuPose3Fuser::readParams ( const ros::NodeHandle & privateNode )

overrideprotectedvirtual

Pose3 Odometry

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 16 of file readParams.cpp.

◆ setup()

void imu_pose3_fuser::ImuPose3Fuser::setup ( )

Definition at line 32 of file ImuPose3Fuser.cpp.

Member Data Documentation

◆ currentTime_

std::chrono::time_point<std::chrono::high_resolution_clock> imu_pose3_fuser::ImuPose3Fuser::currentTime_

private

Definition at line 48 of file ImuPose3Fuser.h.

◆ measPose3_worldImuPathPtr_

nav_msgs::PathPtr imu_pose3_fuser::ImuPose3Fuser::measPose3_worldImuPathPtr_

private

Definition at line 72 of file ImuPose3Fuser.h.

◆ pose3OdometryRate_

double imu_pose3_fuser::ImuPose3Fuser::pose3OdometryRate_ = 5.0

private

Definition at line 53 of file ImuPose3Fuser.h.

◆ pose3UnaryNoise_

Eigen::Matrix<double, 6, 1> imu_pose3_fuser::ImuPose3Fuser::pose3UnaryNoise_

private

Definition at line 56 of file ImuPose3Fuser.h.

◆ pubMeasPose3Path_

ros::Publisher imu_pose3_fuser::ImuPose3Fuser::pubMeasPose3Path_

private

Definition at line 69 of file ImuPose3Fuser.h.

◆ startTime_

std::chrono::time_point<std::chrono::high_resolution_clock> imu_pose3_fuser::ImuPose3Fuser::startTime_

private

Definition at line 47 of file ImuPose3Fuser.h.

◆ subPose3Odometry_

ros::Subscriber imu_pose3_fuser::ImuPose3Fuser::subPose3Odometry_

private

Definition at line 65 of file ImuPose3Fuser.h.

◆ unaryPose3Frame_

std::string imu_pose3_fuser::ImuPose3Fuser::unaryPose3Frame_

private

Definition at line 75 of file ImuPose3Fuser.h.

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

ros/examples/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h
ros/examples/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp
ros/examples/imu_pose3_fuser/src/lib/readParams.cpp

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ImuPose3Fuser()

◆ ~ImuPose3Fuser()

Member Function Documentation

◆ initializeMessages()

◆ initializePublishers()

◆ initializeServices()

◆ initializeSubscribers()

◆ pose3Callback_()

◆ readParams()

◆ setup()

Member Data Documentation

◆ currentTime_

◆ measPose3_worldImuPathPtr_

◆ pose3OdometryRate_

◆ pose3UnaryNoise_

◆ pubMeasPose3Path_

◆ startTime_

◆ subPose3Odometry_

◆ unaryPose3Frame_