8#ifndef HOLISTIC_FUSION_H
9#define HOLISTIC_FUSION_H
16#include <unordered_set>
19#include <gtsam/geometry/Pose3.h>
45 void setup(
const std::shared_ptr<GraphConfig> graphConfigPtr,
const std::shared_ptr<StaticTransforms> staticTransformsPtr);
69 std::optional<BatchOptimizationResult>
optimizeSlowBatchSmoother(
int maxIterations,
const bool computePoseCovarianceFlag);
82 std::shared_ptr<SafeIntegratedNavState>& returnPreIntegratedNavStatePtr,
83 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& returnOptimizedStateWithCovarianceAndBiasPtr,
84 Eigen::Matrix<double, 6, 1>& returnAddedImuMeasurements);
90 const bool addToOnlineSmootherFlag =
true) = 0;
101 const int landmarkCreationCounter) = 0;
115 bool isCovarianceViolated_(
const Eigen::Matrix<double, DIM, 1>& covariance,
const double covarianceViolationThreshold) {
116 if (covarianceViolationThreshold > 0.0) {
117 for (
int i = 0; i < DIM; i++) {
118 if (covariance(i) > covarianceViolationThreshold) {
165 bool alignImu_(
double& imuAttitudeRoll,
double& imuAttitudePitch);
167 void initGraph_(
const double timeStamp_k,
const Eigen::Vector3d& imuAngularVelocity);
182 const Eigen::Matrix3d& R_W_frame2);
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.
bool addZeroMotionFactor(double timeKm1, double timeK, double noiseDensity)
void setup(const std::shared_ptr< GraphConfig > graphConfigPtr, const std::shared_ptr< StaticTransforms > staticTransformsPtr)
std::shared_ptr< GraphConfig > graphConfigPtr_
std::shared_ptr< SafeIntegratedNavState > preIntegratedNavStatePtr_
State Containers.
virtual void addUnaryVelocity3LocalMeasurement(UnaryMeasurementXD< Eigen::Vector3d, 3 > &S_v_R_S)=0
bool initYawAndPosition(const UnaryMeasurementXDAbsolute< double, 1 > &yaw_W_S1, const UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &M_t_M_S2)
std::atomic< bool > stopOptimizeGraphThread_
void setInitialOrientationAndPosition_(const gtsam::Rot3 &R_W_I, const Eigen::Vector3d &W_t_W_S, const std::string &sensorFrame)
bool normalOperationFlag_
bool initWorldFrameToFixedFrameTransform(const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
virtual void addBinaryPose3Measurement(const BinaryMeasurementXD< Eigen::Isometry3d, 6 > &delta)=0
Binary Measurements.
virtual void addUnaryPosition3LandmarkMeasurement(UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_t_S_L, const int landmarkCreationCounter)=0
Landmark Measurements.
bool logRealTimeNavStates(const std::string &savePath)
std::shared_ptr< holistic_fusion::ImuBuffer > coreImuBufferPtr_
virtual void addUnaryVelocity3AbsoluteMeasurement(UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_v_R_S)=0
gtsam::Pose3 initialT_W_fixedFrame_(const UnaryMeasurementAbsolute &measurement)
bool alignedImuFlag_
Flags / Initialization.
bool areYawAndPositionInited() const
bool foundInitialYawAndPositionFlag_
bool addZeroVelocityFactor(double timeK, double noiseDensity)
bool validFirstMeasurementReceivedFlag_
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 alignImu_(double &imuAttitudeRoll, double &imuAttitudePitch)
Worker functions / Set Imu Attitude.
virtual void addUnaryBearing3LandmarkMeasurement(UnaryMeasurementXDLandmark< Eigen::Vector3d, 3 > &S_bearing_S_L)=0
virtual ~HolisticFusion()
Destructor --------—.
std::thread optimizeGraphThread_
std::shared_ptr< GraphManager > graphMgrPtr_
virtual void addUnaryYawAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &yaw_R_S)=0
bool canInitYawAndPosition_() const
Initialization / Whether the yaw and the position can be initialized now. Prints the reason if not.
bool optimizeSlowBatchSmoother(int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
bool areRollAndPitchInited() const
std::mutex optimizeGraphMutex_
std::shared_ptr< StaticTransforms > staticTransformsPtr_
bool checkAndPrintCovarianceViolation_(const std::string &measurementName, const bool violatedFlag)
virtual void addUnaryPose3AbsoluteMeasurement(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &R_T_R_S, const bool addToOnlineSmootherFlag=true)=0
Unary Measurements / Absolute Measurements.
void initGraph_(const double timeStamp_k, const Eigen::Vector3d &imuAngularVelocity)
bool initHeadingAndPosition(const UnaryMeasurementXDAbsolute< Eigen::Isometry3d, 6 > &T_M_S)
virtual void addUnaryPosition3AbsoluteMeasurement(UnaryMeasurementXDAbsolute< Eigen::Vector3d, 3 > &R_t_R_S)=0
bool isCovarianceViolated_(const Eigen::Matrix< double, DIM, 1 > &covariance, const double covarianceViolationThreshold)
Convenience functions.
HolisticFusion()
Constructor --------—.
bool getNormalOperationFlag() const
bool initHeadingAndPositionAsIdentity()
void pretendFirstMeasurementReceived()
std::unordered_set< std::string > measurementsWithViolatedCovariance_
std::mutex initYawAndPositionMutex_
virtual void addUnaryPitchAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &pitch_R_S)=0
virtual void addUnaryRollAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &roll_R_S)=0
bool isGraphInited() const
Class to represent an absolute unary measurement in the graph. Exact dimension is not known.
Class to represent x-dimensional absolute unary measurement in the graph.
Class to represent x-dimensional landmark unary measurement in the graph.
Class to represent x-dimensional unary measurement in the graph.