My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
HolisticFusion.h
Go to the documentation of this file.
1/*
2Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
3All rights reserved.
4This file is released under the "BSD-3-Clause License".
5Please see the LICENSE file that has been included as part of this package.
6 */
7
8#ifndef HOLISTIC_FUSION_H
9#define HOLISTIC_FUSION_H
10
11// C++
12#include <atomic>
13#include <mutex>
14#include <stdexcept>
15#include <thread>
16#include <unordered_set>
17
18// GTSAM
19#include <gtsam/geometry/Pose3.h>
20
21// Package
30
31namespace holistic_fusion {
32
33// Forward Declarations
34class GraphManager;
35
36// Actual Class
38 public: // Interface
39 // Constructor
41 // Destructor
42 virtual ~HolisticFusion();
43
44 // Setup
45 void setup(const std::shared_ptr<GraphConfig> graphConfigPtr, const std::shared_ptr<StaticTransforms> staticTransformsPtr);
46
47 // Initialization Interface
48 // The graph does not start before one of the three init functions succeeds. Each returns false until the IMU is aligned
49 // (areRollAndPitchInited()) and after the first success. Roll and pitch always come from the IMU alignment.
50 // A measurement in a fixed frame M other than the world goes through the guess of T_W_M from
51 // initWorldFrameToFixedFrameTransform(), or identity if none is set, so that the world starts at M.
52 // Sets the heading and the position of the state so that sensor frame S matches the measured pose. Like the heading factor, the
53 // heading is the rotation about the z-axis of the world, so it is defined for every attitude.
55 // Sets the yaw and the position of the state so that the yaw of sensor frame S1 and the position of sensor frame S2 match. Like the
56 // yaw factor, the yaw is in the world frame, and a yaw in another fixed frame throws std::invalid_argument. It is undefined when
57 // the x-axis of S1 is vertical.
60 // Without an absolute reference: sets the heading and the position of the initialization frame in the world to identity.
62 bool initWorldFrameToFixedFrameTransform(const Eigen::Isometry3d& T_W_F, const std::string& fixedFrame);
63
64 // Trigger offline smoother optimization
65 // Writes the optimized states to files in savePath. Returns false if the slow batch smoother is disabled.
66 bool optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag);
67 // Returns the optimized navigation states without writing files. Returns std::nullopt if the slow batch smoother is disabled.
68 // Computing the pose covariances is slow for long trajectories.
69 std::optional<BatchOptimizationResult> optimizeSlowBatchSmoother(int maxIterations, const bool computePoseCovarianceFlag);
70
71 // Logging of the real-time states
72 bool logRealTimeNavStates(const std::string& savePath);
73
74 // Getter functions
75 bool areYawAndPositionInited() const;
76 bool areRollAndPitchInited() const;
77 bool isGraphInited() const;
79
80 // Main: IMU
81 bool addCoreImuMeasurementAndGetState(const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, const double imuTimeK,
82 std::shared_ptr<SafeIntegratedNavState>& returnPreIntegratedNavStatePtr,
83 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& returnOptimizedStateWithCovarianceAndBiasPtr,
84 Eigen::Matrix<double, 6, 1>& returnAddedImuMeasurements);
85
86 // Pure Virtual Methods
90 const bool addToOnlineSmootherFlag = true) = 0;
98
101 const int landmarkCreationCounter) = 0;
103
106
107 // Ambiguous Measurements
108 bool addZeroMotionFactor(double timeKm1, double timeK, double noiseDensity);
109 bool addZeroVelocityFactor(double timeK, double noiseDensity);
110
111 protected:
112 // Methods -------------
114 template <int DIM>
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) {
119 return true;
120 }
121 }
122 }
123 return false;
124 }
125
126 // Initialization
128
129 // Check whether measurement violated covariance, if yes, add to set and print once, if not, remove from set and print that returned
130 bool checkAndPrintCovarianceViolation_(const std::string& measurementName, const bool violatedFlag);
131
132 // Members
133 // Graph Config
134 std::shared_ptr<GraphConfig> graphConfigPtr_ = nullptr;
135 // Extrinsics
136 std::shared_ptr<StaticTransforms> staticTransformsPtr_ = nullptr;
137
138 // Factor graph
139 std::shared_ptr<GraphManager> graphMgrPtr_ = nullptr;
140 // Imu Buffer
141 std::shared_ptr<holistic_fusion::ImuBuffer> coreImuBufferPtr_;
142
143 // Mutex
145
148 bool alignedImuFlag_ = false;
150 bool initedGraphFlag_ = false;
153 bool optimizeGraphFlag_ = false;
155
156 // Counter
158
159 // Set of measurements that have violated the covariance
160 std::unordered_set<std::string> measurementsWithViolatedCovariance_;
161
162 private:
165 bool alignImu_(double& imuAttitudeRoll, double& imuAttitudePitch);
167 void initGraph_(const double timeStamp_k, const Eigen::Vector3d& imuAngularVelocity);
169 void optimizeGraph_();
170
173 bool canInitYawAndPosition_() const;
175 gtsam::Pose3 initialT_W_fixedFrame_(const UnaryMeasurementAbsolute& measurement);
177 void setInitialOrientationAndPosition_(const gtsam::Rot3& R_W_I, const Eigen::Vector3d& W_t_W_S, const std::string& sensorFrame);
178
181 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,
182 const Eigen::Matrix3d& R_W_frame2);
183
184 // Threads
187 std::atomic<bool> stopOptimizeGraphThread_{false};
188
189 // Mutex
191
193 std::shared_ptr<SafeIntegratedNavState> preIntegratedNavStatePtr_ = nullptr;
194};
195
196} // namespace holistic_fusion
197
198#endif // HOLISTIC_FUSION_H
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 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 addZeroVelocityFactor(double timeK, double noiseDensity)
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::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)
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 --------—.
std::unordered_set< std::string > measurementsWithViolatedCovariance_
virtual void addUnaryPitchAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &pitch_R_S)=0
virtual void addUnaryRollAbsoluteMeasurement(const UnaryMeasurementXDAbsolute< double, 1 > &roll_R_S)=0
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.