93 this, ros::TransportHints().tcpNoDelay());
113 REGULAR_COUT <<
RED_START <<
" IMU callback is setting global yaw and position, as no other odometry is available. Initializing..."
127 static int lidarOdometryCallbackCounter__ = -1;
130 ++lidarOdometryCallbackCounter__;
132 Eigen::Isometry3d lio_T_M_Lk;
136 double lidarOdometryTimeK = odomLidarPtr->header.stamp.toSec();
148 if (lidarOdometryCallbackCounter__ <= 2) {
180 Eigen::Isometry3d T_O_Bw_k;
182 double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.toSec();
192 const std::string& wheelOdometryFrame =
202 REGULAR_COUT <<
" Initialized yaw and position to identity in the wheel odometry callback, as lio and vio are all set to false."
211 Eigen::Isometry3d T_Bkm1_Bk =
T_O_Bw_km1_.inverse() * T_O_Bw_k;
232 const double timeK = wheelsSpeedsPtr->data[0];
233 const double leftWheelSpeedRps = wheelsSpeedsPtr->data[1];
234 const double rightWheelSpeedRps = wheelsSpeedsPtr->data[2];
240 REGULAR_COUT <<
" Wheel linear velocities callback: " << std::setprecision(14) << timeK <<
" " << leftWheelSpeedMs <<
" [m/s] "
241 << rightWheelSpeedMs <<
" [m/s]" << std::endl;
245 const std::string& wheelLinearVelocityLeftFrame =
247 const std::string& wheelLinearVelocityRightFrame =
254 REGULAR_COUT <<
GREEN_START <<
" Wheel linear velocities callback is setting global yaw and position, as lio is all set to false."
258 <<
" Initialized yaw and position to identity in the wheel linear velocities callback, as lio and vio are all set to false."
279 std::cout <<
"VIO odometry not yet stable enough for usage, disable flag." << std::endl;
282 Eigen::Isometry3d vio_T_M_Ck;
void addBinaryPose3Measurement(const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &F_T_F_S) override
Binary Measurements.
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 addUnaryVelocity3LocalMeasurement(UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_F_S) override
std::string sensorFrameCorrectedNameId
static void addToPathMsg(const nav_msgs::PathPtr &pathPtr, const std::string &frameName, const ros::Time &stamp, const Eigen::Vector3d &t, int maxBufferLength)
ros::NodeHandle privateNode
virtual void imuCallback(const sensor_msgs::Imu::ConstPtr &imuPtr)
HolisticFusionRos(const std::shared_ptr< ros::NodeHandle > &privateNodePtr)
std::shared_ptr< GraphConfig > graphConfigPtr_
bool areYawAndPositionInited() const
bool areRollAndPitchInited() const
std::shared_ptr< StaticTransforms > staticTransformsPtr_
bool initHeadingAndPosition(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
bool initHeadingAndPositionAsIdentity()
void pretendFirstMeasurementReceived()
Class to represent x-dimensional absolute unary measurement in the graph.
Class to represent x-dimensional unary measurement in the graph.
void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr &lidarOdomPtr)
void initializeSubscribers(ros::NodeHandle &privateNode) override
double wheelOdometryTimeKm1_
ros::Subscriber subVioOdometry_
Eigen::Matrix< double, 6, 1 > initialSe3AlignmentNoise_
void initializeMessages(ros::NodeHandle &privateNode) override
double wheelLinearVelocitiesRate_
bool useWheelOdometryBetweenFlag_
nav_msgs::PathPtr measVio_mapImuPathPtr_
void initializePublishers(ros::NodeHandle &privateNode) override
Eigen::Matrix< double, 6, 1 > lioPoseUnaryNoise_
double wheelOdometryBetweenRate_
int wheelOdometryCallbackCounter_
void wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr &wheelOdomPtr)
Eigen::Matrix< double, 6, 1 > lioSe3AlignmentRandomWalk_
ros::Subscriber subLioOdometry_
ros::Subscriber subWheelOdometryBetween_
void readParams(const ros::NodeHandle &privateNode) override
Eigen::Matrix< double, 3, 1 > wheelLinearVelocitiesNoise_
void initializeServices(ros::NodeHandle &privateNode) override
void vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr &vioOdomPtr)
SmbEstimator(std::shared_ptr< ros::NodeHandle > privateNodePtr)
ros::Publisher pubMeasMapVioPath_
bool useWheelLinearVelocitiesFlag_
ros::Publisher pubMeasMapLioPath_
Eigen::Matrix< double, 6, 1 > wheelPoseBetweenNoise_
void wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr &wheelsSpeedsPtr)
ros::Subscriber subWheelLinearVelocities_
Eigen::Isometry3d T_O_Bw_km1_
void imuCallback(const sensor_msgs::Imu::ConstPtr &imuPtr) override
nav_msgs::PathPtr measLio_mapImuPathPtr_
void odomMsgToEigen(const nav_msgs::Odometry &odomLidar, Eigen::Matrix4d &T)
static RobustNorm Tukey(const double &robustNormConstant)