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

#include <Position3Estimator.h>

Inheritance diagram for position3_se::Position3Estimator:
Inheritance graph
[legend]
Collaboration diagram for position3_se::Position3Estimator:
Collaboration graph
[legend]

Public Member Functions

 Position3Estimator (std::shared_ptr< ros::NodeHandle > privateNodePtr)
 ~Position3Estimator ()=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
bool srvOfflineSmootherOptimizeCallback (holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request &req, holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response &res) override
Protected Member Functions inherited from holistic_fusion::HolisticFusionRos
virtual void imuCallback (const sensor_msgs::Imu::ConstPtr &imuPtr)
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 prismPositionCallback_ (const geometry_msgs::PointStamped::ConstPtr &leicaPositionPtr)
void gnssPositionCallback_ (const sensor_msgs::NavSatFix::ConstPtr &gnssPositionPtr)
void gnssOfflinePoseCallback_ (const nav_msgs::Odometry::ConstPtr &gnssOfflinePosePtr)
bool srvTogglePrismUnaryCallback_ (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
bool srvToggleGnssUnaryCallback_ (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
bool srvToggleGnssOfflinePoseUnaryCallback_ (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)

Private Attributes

ros::Publisher pubMeasWorldPrismPositionPath_
ros::Publisher pubMeasWorldGnssPositionPath_
ros::Publisher pubMeasWorldGnssOfflinePosePath_
nav_msgs::PathPtr measPosition_worldPrismPositionPathPtr_
nav_msgs::PathPtr measPosition_worldGnssPositionPathPtr_
nav_msgs::PathPtr measPosition_worldGnssOfflinePosePathPtr_
ros::ServiceServer srvTogglePrismPositionUnary_
ros::ServiceServer srvToggleGnssPositionUnary_
ros::ServiceServer srvToggleGnssOfflinePoseUnary_
ros::Subscriber subPrismPosition_
ros::Subscriber subGnssPosition_
ros::Subscriber subGnssOfflinePose_
tf::TransformListener tfListener_
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_
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentNoise_ = 10 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 > gnssSe3AlignmentRandomWalk_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
Eigen::Matrix< double, 6, 1 > prismSe3AlignmentRandomWalk_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
std::shared_ptr< holistic_fusion::TrajectoryAlignmentHandler > trajectoryAlignmentHandler_
double prismPositionRate_ = 20.0
double gnssPositionRate_ = 10.0
double gnssOfflinePoseRate_ = 10.0
double prismPositionMeasUnaryNoise_ = 1.0
double gnssPositionMeasUnaryNoise_ = 1.0
Eigen::Matrix< double, 6, 1 > gnssOfflinePoseMeasUnaryNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
bool initializeUsingGnssFlag_ = false
int prismPositionCallbackCounter_ = 0
Eigen::Vector3d initialPrismPosition_ = Eigen::Vector3d::Zero()
bool prismMovedEnoughFlag_ = false
Eigen::Vector3d accumulatedGnssCoordinates_ {0.0, 0.0, 0.0}
int gnssPositionCallbackCounter_ = 0
int gnssOfflinePoseCallbackCounter_ = 0
bool alignedPrismAndGnssFlag_ = false
Eigen::Isometry3d T_totalStation_enu_ = Eigen::Isometry3d::Identity()
Eigen::Isometry3d T_totalStation_totalStationOld_ = Eigen::Isometry3d::Identity()
std::string totalStationReferenceFrame_ = ""
std::string gnssReferenceFrame_ = ""
bool usePrismPositionUnaryFlag_ = constexprUsePrismPositionUnaryFlag_
bool useGnssPositionUnaryFlag_ = constexprUseGnssPositionUnaryFlag_
bool useGnssOfflinePoseUnaryFlag_ = constexprUseGnssOfflinePoseUnaryFlag_

Static Private Attributes

static constexpr double PRISM_MOVED_ENOUGH_THRESHOLD = 1.0
static constexpr bool constexprUsePrismPositionUnaryFlag_ = true
static constexpr bool constexprUseGnssPositionUnaryFlag_ = false
static constexpr bool constexprUseGnssOfflinePoseUnaryFlag_ = 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 40 of file Position3Estimator.h.

Constructor & Destructor Documentation

◆ Position3Estimator()

position3_se::Position3Estimator::Position3Estimator ( std::shared_ptr< ros::NodeHandle > privateNodePtr)

Definition at line 25 of file Position3Estimator.cpp.

◆ ~Position3Estimator()

position3_se::Position3Estimator::~Position3Estimator ( )
default

Member Function Documentation

◆ gnssOfflinePoseCallback_()

void position3_se::Position3Estimator::gnssOfflinePoseCallback_ ( const nav_msgs::Odometry::ConstPtr & gnssOfflinePosePtr)
private

Definition at line 382 of file Position3Estimator.cpp.

◆ gnssPositionCallback_()

void position3_se::Position3Estimator::gnssPositionCallback_ ( const sensor_msgs::NavSatFix::ConstPtr & gnssPositionPtr)
private

Definition at line 312 of file Position3Estimator.cpp.

◆ initializeMessages()

void position3_se::Position3Estimator::initializeMessages ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 113 of file Position3Estimator.cpp.

◆ initializePublishers()

void position3_se::Position3Estimator::initializePublishers ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 73 of file Position3Estimator.cpp.

◆ initializeServices()

void position3_se::Position3Estimator::initializeServices ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 124 of file Position3Estimator.cpp.

◆ initializeSubscribers()

void position3_se::Position3Estimator::initializeSubscribers ( ros::NodeHandle & privateNode)
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 90 of file Position3Estimator.cpp.

◆ prismPositionCallback_()

void position3_se::Position3Estimator::prismPositionCallback_ ( const geometry_msgs::PointStamped::ConstPtr & leicaPositionPtr)
private

Definition at line 194 of file Position3Estimator.cpp.

◆ readParams()

void position3_se::Position3Estimator::readParams ( const ros::NodeHandle & privateNode)
overrideprotectedvirtual

Position Measurment frame (Prism)

Position Measurment frame (GNSS)

Position Measurment frame (GNSS Offline Pose)

Noise Parameters -— Position measurement unary noise

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 17 of file readParams.cpp.

◆ setup()

void position3_se::Position3Estimator::setup ( )

Definition at line 44 of file Position3Estimator.cpp.

◆ srvOfflineSmootherOptimizeCallback()

bool position3_se::Position3Estimator::srvOfflineSmootherOptimizeCallback ( holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Request & req,
holistic_fusion_ros_msgs::OfflineOptimizationTrigger::Response & res )
overrideprotectedvirtual

Reimplemented from holistic_fusion::HolisticFusionRos.

Definition at line 143 of file Position3Estimator.cpp.

◆ srvToggleGnssOfflinePoseUnaryCallback_()

bool position3_se::Position3Estimator::srvToggleGnssOfflinePoseUnaryCallback_ ( std_srvs::Trigger::Request & req,
std_srvs::Trigger::Response & res )
private

Definition at line 484 of file Position3Estimator.cpp.

◆ srvToggleGnssUnaryCallback_()

bool position3_se::Position3Estimator::srvToggleGnssUnaryCallback_ ( std_srvs::Trigger::Request & req,
std_srvs::Trigger::Response & res )
private

Definition at line 472 of file Position3Estimator.cpp.

◆ srvTogglePrismUnaryCallback_()

bool position3_se::Position3Estimator::srvTogglePrismUnaryCallback_ ( std_srvs::Trigger::Request & req,
std_srvs::Trigger::Response & res )
private

Definition at line 460 of file Position3Estimator.cpp.

Member Data Documentation

◆ accumulatedGnssCoordinates_

Eigen::Vector3d position3_se::Position3Estimator::accumulatedGnssCoordinates_ {0.0, 0.0, 0.0}
private

Definition at line 136 of file Position3Estimator.h.

◆ alignedPrismAndGnssFlag_

bool position3_se::Position3Estimator::alignedPrismAndGnssFlag_ = false
private

Definition at line 139 of file Position3Estimator.h.

◆ constexprUseGnssOfflinePoseUnaryFlag_

bool position3_se::Position3Estimator::constexprUseGnssOfflinePoseUnaryFlag_ = true
staticconstexprprivate

Definition at line 150 of file Position3Estimator.h.

◆ constexprUseGnssPositionUnaryFlag_

bool position3_se::Position3Estimator::constexprUseGnssPositionUnaryFlag_ = false
staticconstexprprivate

Definition at line 149 of file Position3Estimator.h.

◆ constexprUsePrismPositionUnaryFlag_

bool position3_se::Position3Estimator::constexprUsePrismPositionUnaryFlag_ = true
staticconstexprprivate

Definition at line 148 of file Position3Estimator.h.

◆ currentTime_

std::chrono::time_point<std::chrono::high_resolution_clock> position3_se::Position3Estimator::currentTime_
private

Definition at line 103 of file Position3Estimator.h.

◆ gnssHandlerPtr_

std::shared_ptr<holistic_fusion::GnssHandler> position3_se::Position3Estimator::gnssHandlerPtr_
private

Definition at line 106 of file Position3Estimator.h.

◆ gnssOfflinePoseCallbackCounter_

int position3_se::Position3Estimator::gnssOfflinePoseCallbackCounter_ = 0
private

Definition at line 138 of file Position3Estimator.h.

◆ gnssOfflinePoseMeasUnaryNoise_

Eigen::Matrix<double, 6, 1> position3_se::Position3Estimator::gnssOfflinePoseMeasUnaryNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 124 of file Position3Estimator.h.

◆ gnssOfflinePoseRate_

double position3_se::Position3Estimator::gnssOfflinePoseRate_ = 10.0
private

Definition at line 119 of file Position3Estimator.h.

◆ gnssPositionCallbackCounter_

int position3_se::Position3Estimator::gnssPositionCallbackCounter_ = 0
private

Definition at line 137 of file Position3Estimator.h.

◆ gnssPositionMeasUnaryNoise_

double position3_se::Position3Estimator::gnssPositionMeasUnaryNoise_ = 1.0
private

Definition at line 123 of file Position3Estimator.h.

◆ gnssPositionRate_

double position3_se::Position3Estimator::gnssPositionRate_ = 10.0
private

Definition at line 118 of file Position3Estimator.h.

◆ gnssReferenceFrame_

std::string position3_se::Position3Estimator::gnssReferenceFrame_ = ""
private

Definition at line 145 of file Position3Estimator.h.

◆ gnssSe3AlignmentRandomWalk_

Eigen::Matrix<double, 6, 1> position3_se::Position3Estimator::gnssSe3AlignmentRandomWalk_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 110 of file Position3Estimator.h.

◆ initializeUsingGnssFlag_

bool position3_se::Position3Estimator::initializeUsingGnssFlag_ = false
private

Definition at line 127 of file Position3Estimator.h.

◆ initialPrismPosition_

Eigen::Vector3d position3_se::Position3Estimator::initialPrismPosition_ = Eigen::Vector3d::Zero()
private

Definition at line 132 of file Position3Estimator.h.

◆ initialSe3AlignmentNoise_

Eigen::Matrix<double, 6, 1> position3_se::Position3Estimator::initialSe3AlignmentNoise_ = 10 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 109 of file Position3Estimator.h.

◆ measPosition_worldGnssOfflinePosePathPtr_

nav_msgs::PathPtr position3_se::Position3Estimator::measPosition_worldGnssOfflinePosePathPtr_
private

Definition at line 86 of file Position3Estimator.h.

◆ measPosition_worldGnssPositionPathPtr_

nav_msgs::PathPtr position3_se::Position3Estimator::measPosition_worldGnssPositionPathPtr_
private

Definition at line 85 of file Position3Estimator.h.

◆ measPosition_worldPrismPositionPathPtr_

nav_msgs::PathPtr position3_se::Position3Estimator::measPosition_worldPrismPositionPathPtr_
private

Definition at line 84 of file Position3Estimator.h.

◆ PRISM_MOVED_ENOUGH_THRESHOLD

double position3_se::Position3Estimator::PRISM_MOVED_ENOUGH_THRESHOLD = 1.0
staticconstexprprivate

Definition at line 134 of file Position3Estimator.h.

◆ prismMovedEnoughFlag_

bool position3_se::Position3Estimator::prismMovedEnoughFlag_ = false
private

Definition at line 133 of file Position3Estimator.h.

◆ prismPositionCallbackCounter_

int position3_se::Position3Estimator::prismPositionCallbackCounter_ = 0
private

Definition at line 131 of file Position3Estimator.h.

◆ prismPositionMeasUnaryNoise_

double position3_se::Position3Estimator::prismPositionMeasUnaryNoise_ = 1.0
private

Definition at line 122 of file Position3Estimator.h.

◆ prismPositionRate_

double position3_se::Position3Estimator::prismPositionRate_ = 20.0
private

Definition at line 117 of file Position3Estimator.h.

◆ prismSe3AlignmentRandomWalk_

Eigen::Matrix<double, 6, 1> position3_se::Position3Estimator::prismSe3AlignmentRandomWalk_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones()
private

Definition at line 111 of file Position3Estimator.h.

◆ pubMeasWorldGnssOfflinePosePath_

ros::Publisher position3_se::Position3Estimator::pubMeasWorldGnssOfflinePosePath_
private

Definition at line 80 of file Position3Estimator.h.

◆ pubMeasWorldGnssPositionPath_

ros::Publisher position3_se::Position3Estimator::pubMeasWorldGnssPositionPath_
private

Definition at line 79 of file Position3Estimator.h.

◆ pubMeasWorldPrismPositionPath_

ros::Publisher position3_se::Position3Estimator::pubMeasWorldPrismPositionPath_
private

Definition at line 78 of file Position3Estimator.h.

◆ srvToggleGnssOfflinePoseUnary_

ros::ServiceServer position3_se::Position3Estimator::srvToggleGnssOfflinePoseUnary_
private

Definition at line 92 of file Position3Estimator.h.

◆ srvToggleGnssPositionUnary_

ros::ServiceServer position3_se::Position3Estimator::srvToggleGnssPositionUnary_
private

Definition at line 91 of file Position3Estimator.h.

◆ srvTogglePrismPositionUnary_

ros::ServiceServer position3_se::Position3Estimator::srvTogglePrismPositionUnary_
private

Definition at line 90 of file Position3Estimator.h.

◆ startTime_

std::chrono::time_point<std::chrono::high_resolution_clock> position3_se::Position3Estimator::startTime_
private

Definition at line 102 of file Position3Estimator.h.

◆ subGnssOfflinePose_

ros::Subscriber position3_se::Position3Estimator::subGnssOfflinePose_
private

Definition at line 97 of file Position3Estimator.h.

◆ subGnssPosition_

ros::Subscriber position3_se::Position3Estimator::subGnssPosition_
private

Definition at line 96 of file Position3Estimator.h.

◆ subPrismPosition_

ros::Subscriber position3_se::Position3Estimator::subPrismPosition_
private

Definition at line 95 of file Position3Estimator.h.

◆ T_totalStation_enu_

Eigen::Isometry3d position3_se::Position3Estimator::T_totalStation_enu_ = Eigen::Isometry3d::Identity()
private

Definition at line 140 of file Position3Estimator.h.

◆ T_totalStation_totalStationOld_

Eigen::Isometry3d position3_se::Position3Estimator::T_totalStation_totalStationOld_ = Eigen::Isometry3d::Identity()
private

Definition at line 141 of file Position3Estimator.h.

◆ tfListener_

tf::TransformListener position3_se::Position3Estimator::tfListener_
private

Definition at line 99 of file Position3Estimator.h.

◆ totalStationReferenceFrame_

std::string position3_se::Position3Estimator::totalStationReferenceFrame_ = ""
private

Definition at line 144 of file Position3Estimator.h.

◆ trajectoryAlignmentHandler_

std::shared_ptr<holistic_fusion::TrajectoryAlignmentHandler> position3_se::Position3Estimator::trajectoryAlignmentHandler_
private

Definition at line 114 of file Position3Estimator.h.

◆ useGnssOfflinePoseUnaryFlag_

bool position3_se::Position3Estimator::useGnssOfflinePoseUnaryFlag_ = constexprUseGnssOfflinePoseUnaryFlag_
private

Definition at line 153 of file Position3Estimator.h.

◆ useGnssPositionUnaryFlag_

bool position3_se::Position3Estimator::useGnssPositionUnaryFlag_ = constexprUseGnssPositionUnaryFlag_
private

Definition at line 152 of file Position3Estimator.h.

◆ usePrismPositionUnaryFlag_

bool position3_se::Position3Estimator::usePrismPositionUnaryFlag_ = constexprUsePrismPositionUnaryFlag_
private

Definition at line 151 of file Position3Estimator.h.

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