My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
excavator_se::ExcavatorEstimator Class Reference

#include <ExcavatorEstimator.h>

Inheritance diagram for excavator_se::ExcavatorEstimator:
Inheritance graph
[legend]
Collaboration diagram for excavator_se::ExcavatorEstimator:
Collaboration graph
[legend]

Public Member Functions

 ExcavatorEstimator (std::shared_ptr< ros::NodeHandle > privateNodePtr)
 ~ExcavatorEstimator ()=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 readParams (const ros::NodeHandle &privateNode) override
virtual void publishState (const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > &preIntegratedNavStatePtr, const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > &optimizedStateWithCovarianceAndBiasPtr) override
Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
virtual void initializeServices (ros::NodeHandle &privateNode)
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 Types

typedef message_filters::sync_policies::ExactTime< sensor_msgs::NavSatFix, sensor_msgs::NavSatFix > GnssExactSyncPolicy

Private Member Functions

void lidarOdometryCallback_ (const nav_msgs::Odometry::ConstPtr &lidar_odom_ptr)
void gnssCallback_ (const sensor_msgs::NavSatFix::ConstPtr &leftGnssPtr, const sensor_msgs::NavSatFix::ConstPtr &rightGnssPtr)

Private Attributes

ros::Publisher pubMeasMapLioPath_
ros::Publisher pubMeasWorldGnssLPath_
ros::Publisher pubMeasWorldGnssRPath_
nav_msgs::PathPtr measLio_mapImuPathPtr_
nav_msgs::PathPtr measGnss_worldGnssLPathPtr_
nav_msgs::PathPtr measGnss_worldGnssRPathPtr_
ros::Subscriber subImu_
ros::Subscriber subLidarOdometry_
message_filters::Subscriber< sensor_msgs::NavSatFix > subGnssLeft_
message_filters::Subscriber< sensor_msgs::NavSatFix > subGnssRight_
tf::TransformListener tfListener_
boost::shared_ptr< message_filters::Synchronizer< GnssExactSyncPolicy > > gnssExactSyncPtr_
std::chrono::time_point< std::chrono::high_resolution_clock > startTime_
std::chrono::time_point< std::chrono::high_resolution_clock > currentTime_
std::shared_ptr< holistic_fusion::GnssHandler > gnssHandlerPtr_
double lioOdometryRate_ = 5.0
double gnssRate_ = 20.0
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentStdDev_ = 10 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 > lioSe3AlignmentRandomWalk_ = 0.01 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 > lioPoseUnaryNoise_
double gnssPositionUnaryNoise_ = 1.0
double gnssHeadingUnaryNoise_ = 1.0
bool useLioOdometryFlag_ = true
 Flags.
bool useLeftGnssFlag_ = true
bool useRightGnssFlag_ = true
bool useGnssYawFlag_ = true

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 37 of file ExcavatorEstimator.h.

Member Typedef Documentation

◆ GnssExactSyncPolicy

typedef message_filters::sync_policies::ExactTime<sensor_msgs::NavSatFix, sensor_msgs::NavSatFix> excavator_se::ExcavatorEstimator::GnssExactSyncPolicy
private

Definition at line 86 of file ExcavatorEstimator.h.

Constructor & Destructor Documentation

◆ ExcavatorEstimator()

excavator_se::ExcavatorEstimator::ExcavatorEstimator ( std::shared_ptr< ros::NodeHandle > privateNodePtr)

Definition at line 22 of file ExcavatorEstimator.cpp.

◆ ~ExcavatorEstimator()

excavator_se::ExcavatorEstimator::~ExcavatorEstimator ( )
default

Member Function Documentation

◆ gnssCallback_()

void excavator_se::ExcavatorEstimator::gnssCallback_ ( const sensor_msgs::NavSatFix::ConstPtr & leftGnssPtr,
const sensor_msgs::NavSatFix::ConstPtr & rightGnssPtr )
private

Definition at line 144 of file ExcavatorEstimator.cpp.

◆ initializeMessages()

void excavator_se::ExcavatorEstimator::initializeMessages ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 87 of file ExcavatorEstimator.cpp.

◆ initializePublishers()

void excavator_se::ExcavatorEstimator::initializePublishers ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 58 of file ExcavatorEstimator.cpp.

◆ initializeSubscribers()

void excavator_se::ExcavatorEstimator::initializeSubscribers ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 68 of file ExcavatorEstimator.cpp.

◆ lidarOdometryCallback_()

void excavator_se::ExcavatorEstimator::lidarOdometryCallback_ ( const nav_msgs::Odometry::ConstPtr & lidar_odom_ptr)
private

Definition at line 97 of file ExcavatorEstimator.cpp.

◆ publishState()

void excavator_se::ExcavatorEstimator::publishState ( const std::shared_ptr< holistic_fusion::SafeIntegratedNavState > & preIntegratedNavStatePtr,
const std::shared_ptr< holistic_fusion::SafeNavStateWithCovarianceAndBias > & optimizedStateWithCovarianceAndBiasPtr )
overrideprotectedvirtual

Definition at line 247 of file ExcavatorEstimator.cpp.

◆ readParams()

void excavator_se::ExcavatorEstimator::readParams ( const ros::NodeHandle & privateNode)
overrideprotectedvirtual

LiDAR frame

Cabin frame

Left Gnss frame

Right Gnss frame

Initial Se3 Alignment

Noise Parameters -— LiDAR Odometry

Gnss

LiDAR Odometry

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 16 of file readParams.cpp.

◆ setup()

void excavator_se::ExcavatorEstimator::setup ( )

Definition at line 36 of file ExcavatorEstimator.cpp.

Member Data Documentation

◆ currentTime_

std::chrono::time_point<std::chrono::high_resolution_clock> excavator_se::ExcavatorEstimator::currentTime_
private

Definition at line 91 of file ExcavatorEstimator.h.

◆ gnssExactSyncPtr_

boost::shared_ptr<message_filters::Synchronizer<GnssExactSyncPolicy> > excavator_se::ExcavatorEstimator::gnssExactSyncPtr_
private

Definition at line 87 of file ExcavatorEstimator.h.

◆ gnssHandlerPtr_

std::shared_ptr<holistic_fusion::GnssHandler> excavator_se::ExcavatorEstimator::gnssHandlerPtr_
private

Definition at line 94 of file ExcavatorEstimator.h.

◆ gnssHeadingUnaryNoise_

double excavator_se::ExcavatorEstimator::gnssHeadingUnaryNoise_ = 1.0
private

Definition at line 107 of file ExcavatorEstimator.h.

◆ gnssPositionUnaryNoise_

double excavator_se::ExcavatorEstimator::gnssPositionUnaryNoise_ = 1.0
private

Definition at line 106 of file ExcavatorEstimator.h.

◆ gnssRate_

double excavator_se::ExcavatorEstimator::gnssRate_ = 20.0
private

Definition at line 98 of file ExcavatorEstimator.h.

◆ initialSe3AlignmentStdDev_

Eigen::Matrix<double, 6, 1> excavator_se::ExcavatorEstimator::initialSe3AlignmentStdDev_ = 10 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 101 of file ExcavatorEstimator.h.

◆ lioOdometryRate_

double excavator_se::ExcavatorEstimator::lioOdometryRate_ = 5.0
private

Definition at line 97 of file ExcavatorEstimator.h.

◆ lioPoseUnaryNoise_

Eigen::Matrix<double, 6, 1> excavator_se::ExcavatorEstimator::lioPoseUnaryNoise_
private

Definition at line 105 of file ExcavatorEstimator.h.

◆ lioSe3AlignmentRandomWalk_

Eigen::Matrix<double, 6, 1> excavator_se::ExcavatorEstimator::lioSe3AlignmentRandomWalk_ = 0.01 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 102 of file ExcavatorEstimator.h.

◆ measGnss_worldGnssLPathPtr_

nav_msgs::PathPtr excavator_se::ExcavatorEstimator::measGnss_worldGnssLPathPtr_
private

Definition at line 75 of file ExcavatorEstimator.h.

◆ measGnss_worldGnssRPathPtr_

nav_msgs::PathPtr excavator_se::ExcavatorEstimator::measGnss_worldGnssRPathPtr_
private

Definition at line 76 of file ExcavatorEstimator.h.

◆ measLio_mapImuPathPtr_

nav_msgs::PathPtr excavator_se::ExcavatorEstimator::measLio_mapImuPathPtr_
private

Definition at line 74 of file ExcavatorEstimator.h.

◆ pubMeasMapLioPath_

ros::Publisher excavator_se::ExcavatorEstimator::pubMeasMapLioPath_
private

Definition at line 68 of file ExcavatorEstimator.h.

◆ pubMeasWorldGnssLPath_

ros::Publisher excavator_se::ExcavatorEstimator::pubMeasWorldGnssLPath_
private

Definition at line 69 of file ExcavatorEstimator.h.

◆ pubMeasWorldGnssRPath_

ros::Publisher excavator_se::ExcavatorEstimator::pubMeasWorldGnssRPath_
private

Definition at line 70 of file ExcavatorEstimator.h.

◆ startTime_

std::chrono::time_point<std::chrono::high_resolution_clock> excavator_se::ExcavatorEstimator::startTime_
private

Definition at line 90 of file ExcavatorEstimator.h.

◆ subGnssLeft_

message_filters::Subscriber<sensor_msgs::NavSatFix> excavator_se::ExcavatorEstimator::subGnssLeft_
private

Definition at line 81 of file ExcavatorEstimator.h.

◆ subGnssRight_

message_filters::Subscriber<sensor_msgs::NavSatFix> excavator_se::ExcavatorEstimator::subGnssRight_
private

Definition at line 82 of file ExcavatorEstimator.h.

◆ subImu_

ros::Subscriber excavator_se::ExcavatorEstimator::subImu_
private

Definition at line 79 of file ExcavatorEstimator.h.

◆ subLidarOdometry_

ros::Subscriber excavator_se::ExcavatorEstimator::subLidarOdometry_
private

Definition at line 80 of file ExcavatorEstimator.h.

◆ tfListener_

tf::TransformListener excavator_se::ExcavatorEstimator::tfListener_
private

Definition at line 83 of file ExcavatorEstimator.h.

◆ useGnssYawFlag_

bool excavator_se::ExcavatorEstimator::useGnssYawFlag_ = true
private

Definition at line 113 of file ExcavatorEstimator.h.

◆ useLeftGnssFlag_

bool excavator_se::ExcavatorEstimator::useLeftGnssFlag_ = true
private

Definition at line 111 of file ExcavatorEstimator.h.

◆ useLioOdometryFlag_

bool excavator_se::ExcavatorEstimator::useLioOdometryFlag_ = true
private

Flags.

Definition at line 110 of file ExcavatorEstimator.h.

◆ useRightGnssFlag_

bool excavator_se::ExcavatorEstimator::useRightGnssFlag_ = true
private

Definition at line 112 of file ExcavatorEstimator.h.

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