My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
GraphManager.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 GRAPH_MANAGER_HPP
9#define GRAPH_MANAGER_HPP
10
11// C++
12#include <chrono>
13#include <mutex>
14#include <vector>
15
16// GTSAM
17#include <gtsam/nonlinear/ExpressionFactorGraph.h>
18#include <gtsam/slam/expressions.h>
19
20// Package
34
35// General Unary Factor Interface
39
40// General Binary Factor Interface
41// TODO: add binary factor interface
42
43namespace holistic_fusion {
44
52 public:
53 GraphManager(std::shared_ptr<GraphConfig> graphConfigPtr, std::string imuFrame, std::string worldFrame);
55 std::cout << YELLOW_START << "HolisticFusion: GraphManager" << GREEN_START << " Destructor called." << COLOR_END << std::endl;
56 if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
57 std::cout << YELLOW_START << "HolisticFusion: GraphManager" << COLOR_END
58 << " Additional slow batch smoother was built up. Next time the optimization of it can be called before shutting down (if "
59 "not done already)."
60 << std::endl;
61 }
62 };
63
64 // Initialization Interface ---------------------------------------------------
65 bool initImuIntegrators(double gravityValue);
66 // imuAngularVelocity is the bias-uncorrected IMU-frame sample at timeStamp [rad/s].
67 bool initPoseVelocityBiasGraph(double timeStamp, const gtsam::Pose3& T_W_I0, const gtsam::Pose3& T_O_I0,
68 const gtsam::Vector3& imuAngularVelocity);
69
70 // IMU at the core -----------------------------------------------------------
71 void addImuFactorAndGetState(SafeIntegratedNavState& returnPreIntegratedNavState,
72 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& newOptimizedNavStatePtr,
73 const std::shared_ptr<ImuBuffer>& imuBufferPtr, double imuTimeK, bool createNewStateFlag);
74
75 // All other measurements -----------------------------------------------------
76
77 // Unary commodity methods --> Key Lookup
78 bool getUnaryFactorGeneralKey(gtsam::Key& returnedKey, double& returnedGraphTime, const UnaryMeasurement& unaryMeasurement);
79 // True if the graph holds an alignment keyframe for the transform from worldFrame to fixedFrame.
80 bool hasReferenceFrameKeyframe(const std::string& worldFrame, const std::string& fixedFrame);
81
82 // Unary Meta Method --> classic GTSAM Factors
83 typedef gtsam::Key (*F)(std::uint64_t);
84 template <class MEASUREMENT_TYPE, int NOISE_DIM, class FACTOR_TYPE, F SYMBOL_SHORTHAND, class... FACTOR_ARGS>
85 // Extra factor constructor arguments follow the noise model.
86 void addUnaryClassicFactor(const MEASUREMENT_TYPE& unaryMeasurement, const Eigen::Matrix<double, NOISE_DIM, 1>& unaryNoiseDensity,
87 double measurementTime, const FACTOR_ARGS&... factorArgs);
88
89 // GMSF Holistic Graph Factors with Extrinsic Calibration ------------------------
90 template <class GMSF_EXPRESSION_TYPE>
91 void addUnaryHolisticFactor(const std::shared_ptr<GMSF_EXPRESSION_TYPE> gmsfUnaryExpressionPtr,
92 const bool addToOnlineSmootherFlag = true);
93
94 // Robust Norm Aware Between Factor
95 gtsam::Key addPoseBetweenFactor(const gtsam::Pose3& deltaPose, const Eigen::Matrix<double, 6, 1>& poseBetweenNoiseDensity,
96 double lidarTimeKm1, double lidarTimeK, double rate, const RobustNormEnum& robustNormEnum,
97 const double robustNormConstant);
98
99 // Set T_W_F
100 bool setInitialWorldFrameToFixedFrameTransform(const Eigen::Isometry3d& T_W_F, const std::string& fixedFrame);
101 // The guess set above, or identity if none is set
102 gtsam::Pose3 getInitialWorldFrameToFixedFrameTransform(const std::string& fixedFrame);
103
104 // Update of graph ----------------------------------------------------------
105 // Real-time Graph Update
106 void updateGraph();
107
108 // Slow Graph Update (if desired)
109 bool optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag);
110 std::optional<BatchOptimizationResult> optimizeSlowBatchSmoother(int maxIterations, const bool computePoseCovarianceFlag);
111
112 // Logging of real-time navigation states
113 bool logRealTimeNavStates(const std::string& savePath, const std::string& timeString);
114
115 // Logging of real-time reference frame states
116 bool logRealTimeReferenceFrameStates(const std::string& savePath, const std::string& timeString);
117
118 // Logging of Latency and Update Duration
119 bool logLatencyAndUpdateDuration(const std::string& savePath, const std::string& timeString);
120
121 // Save Variables to File
122 void saveOptimizedValuesToFile(const gtsam::Values& optimizedValues, const std::map<gtsam::Key, double>& keyTimestampMap,
123 const std::string& savePath, const bool saveCovarianceFlag);
124
125 // Save Optimized Graph to G2O Format
126 static void saveOptimizedGraphToG2o(const OptimizerBase& optimizedGraph, const gtsam::Values& optimizedValues,
127 const std::string& saveFileName);
128
129 // Comfort functions ---------------------------------------------------------
130 gtsam::NavState calculateStateAtGeneralKey(bool& computeSuccessfulFlag, const gtsam::Key& generalKey);
131
132 // Accessors
134 Eigen::Vector3d& getInitAccBiasReference() { return graphConfigPtr_->accBiasPrior_; }
135 Eigen::Vector3d& getInitGyrBiasReference() { return graphConfigPtr_->gyroBiasPrior_; }
136
137 // auto iterations() const { return additonalIterations_; }
139 const gtsam::Key getPropagatedStateKey() { return propagatedStateKey_; }
140
141 protected:
142 // Calculate state at key for graph
143 static gtsam::NavState calculateNavStateAtGeneralKey(bool& computeSuccessfulFlag, std::shared_ptr<holistic_fusion::OptimizerBase> graphPtr,
144 const gtsam::Key& generalKey, const char* callingFunctionName);
145
146 // Get Covariance of Pose at Key in World Frame, with optional NavState
147 static Eigen::Matrix<double, 6, 6> calculatePoseCovarianceAtKeyInWorldFrame(
148 std::shared_ptr<holistic_fusion::OptimizerBase> graphPtr, const gtsam::Key& graphKey, const char* callingFunctionName,
149 std::optional<const gtsam::NavState> optionalNavState = std::nullopt);
150
151 private:
152 // Methods
153 // Returns false if the slow batch smoother is disabled.
154 bool runSlowBatchSmootherOptimization_(int maxIterations);
155 BatchOptimizationResult extractBatchOptimizationResult_(const bool computePoseCovarianceFlag);
156 template <class CHILDPTR>
157 bool addFactorToRtAndBatchGraph_(const gtsam::NoiseModelFactor* noiseModelFactorPtr, const bool addToOnlineSmootherFlag = true);
158 template <class CHILDPTR>
159 bool addFactorToRtAndBatchGraph_(const gtsam::NoiseModelFactor* noiseModelFactorPtr, double measurementTimestamp,
160 const std::string& measurementName, const bool addToOnlineSmootherFlag = true);
161 template <class CHILDPTR>
162 bool addFactorSafelyToRtAndBatchGraph_(const gtsam::NoiseModelFactor* noiseModelFactorPtr, double measurementTimestamp);
164 void updateImuIntegrators_(const TimeToImuMap& imuMeas);
165
166 // Add Factors, Values, and Keys to Graph
167 // Rt Smoother
168 bool addFactorsToRtSmootherAndOptimize(const gtsam::NonlinearFactorGraph& newRtGraphFactors, const gtsam::Values& newRtGraphValues,
169 const std::map<gtsam::Key, double>& newRtGraphKeysTimestampsMap,
170 const std::shared_ptr<GraphConfig>& graphConfigPtr, const int additionalIterations);
171 // Batch Smoother
172 bool addFactorsToBatchSmootherAndOptimize(const gtsam::NonlinearFactorGraph& newBatchGraphFactors,
173 const gtsam::Values& newBatchGraphValues,
174 const std::map<gtsam::Key, double>& newBatchGraphKeysTimestampsMap,
175 const std::shared_ptr<GraphConfig>& graphConfigPtr);
177 bool findGraphKeys_(gtsam::Key& closestKeyKm1, gtsam::Key& closestKeyK, double& keyTimeStampDistance, double maxTimestampDistance,
178 double timeKm1, double timeK, const std::string& name);
180 const uint64_t newPropagatedStateKey_() { return ++propagatedStateKey_; }
182 inline void writeKeyToKeyTimeStampMap_(const gtsam::Key& key, double measurementTime,
183 std::shared_ptr<std::map<gtsam::Key, double>> keyTimestampMapPtr);
184
185 void writeValueKeysToKeyTimeStampMap_(const gtsam::Values& values, double measurementTime,
186 std::shared_ptr<std::map<gtsam::Key, double>> keyTimestampMapPtr);
187
188 // Buffers
189 std::shared_ptr<TimeGraphKeyBuffer> timeToKeyBufferPtr_;
190
191 // Optimization Transformations
192 std::string imuFrame_;
193 std::string worldFrame_;
195
196 // File Logger
198
199 // Objects
200 std::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> imuParamsPtr_;
201 std::shared_ptr<gtsam::imuBias::ConstantBias> imuBiasPriorPtr_;
204 gtsam::NavState W_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0));
205 gtsam::NavState O_imuPropagatedState_ = gtsam::NavState(gtsam::Pose3(), gtsam::Vector3(0, 0, 0));
206 Eigen::Isometry3d T_W_O_ = Eigen::Isometry3d::Identity(); // Current state pose, depending on whether propagated state jumps or not
207 gtsam::Key propagatedStateKey_ = 0; // Current state key, always start with 0
210 double lastOptimizedStateTime_ = 0.0; // Last optimized state time
211 gtsam::Vector3 latestGraphStateKeyAngularVelocity_ = gtsam::Vector3(0, 0, 0);
212
213 // Optimizer(s)
214 std::shared_ptr<OptimizerBase> rtOptimizerPtr_;
215 std::shared_ptr<OptimizerBase> batchOptimizerPtr_;
217 std::shared_ptr<gtsam::NonlinearFactorGraph> rtFactorGraphBufferPtr_;
218 std::shared_ptr<gtsam::NonlinearFactorGraph> batchFactorGraphBufferPtr_;
219 // Values map
220 std::shared_ptr<gtsam::Values> rtGraphValuesBufferPtr_;
221 std::shared_ptr<gtsam::Values> batchGraphValuesBufferPtr_;
222 // Keys timestamp map
223 std::shared_ptr<std::map<gtsam::Key, double>> rtGraphKeysTimestampsMapBufferPtr_;
224 std::shared_ptr<std::map<gtsam::Key, double>> batchGraphKeysTimestampsMapBufferPtr_;
225
226 // Preintegration
228 std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> imuStepPreintegratorPtr_;
230 std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> imuBufferPreintegratorPtr_;
231
233 gtsam::Vector6 lastImuVector_;
234
236 std::shared_ptr<holistic_fusion::GraphConfig> graphConfigPtr_ = nullptr;
237
238 // Real-time Pose Container
239 std::map<double, gtsam::Pose3> realTimeWorldPoseContainer_ = {};
240 std::map<double, gtsam::Pose3> realTimeOdomPoseContainer_ = {};
241 // Real-time Reference Frame Transformation Container
242 std::map<double, TransformsDictionary<Eigen::Isometry3d>> realTimeReferenceFrameContainer_ = {};
243 std::set<std::pair<std::string, std::string>> realTimeReferenceFrameNamePairs_ = {};
244 // Latency Start and End Time
245 std::chrono::time_point<std::chrono::high_resolution_clock> latencyStartTime_;
246 std::chrono::time_point<std::chrono::high_resolution_clock> latencyEndTime_;
247 // Latency Container
248 std::map<double, double> latencyContainer_ = {};
249 // Update Duration Start and End Time
250 std::chrono::time_point<std::chrono::high_resolution_clock> updateDurationStartTime_;
251 std::chrono::time_point<std::chrono::high_resolution_clock> updateDurationEndTime_;
252 // Update Duration Container
253 std::map<double, double> updateDurationContainer_ = {};
254
255 // Member variables
259};
260
261} // namespace holistic_fusion
262
263// Template Implementations
264#include "holistic_fusion/core/GraphManager.inl"
265
266#endif // GRAPH_MANAGER_HPP
#define COLOR_END
#define GREEN_START
#define YELLOW_START
const uint64_t newPropagatedStateKey_()
Generate new key.
std::shared_ptr< TimeGraphKeyBuffer > timeToKeyBufferPtr_
gtsam::NavState O_imuPropagatedState_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements > imuStepPreintegratorPtr_
Step Preintegrator.
void saveOptimizedValuesToFile(const gtsam::Values &optimizedValues, const std::map< gtsam::Key, double > &keyTimestampMap, const std::string &savePath, const bool saveCovarianceFlag)
Eigen::Vector3d & getInitAccBiasReference()
Getters.
std::set< std::pair< std::string, std::string > > realTimeReferenceFrameNamePairs_
std::map< double, gtsam::Pose3 > realTimeOdomPoseContainer_
const GraphState & getOptimizedGraphState()
bool logLatencyAndUpdateDuration(const std::string &savePath, const std::string &timeString)
void addImuFactorAndGetState(SafeIntegratedNavState &returnPreIntegratedNavState, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &newOptimizedNavStatePtr, const std::shared_ptr< ImuBuffer > &imuBufferPtr, double imuTimeK, bool createNewStateFlag)
bool setInitialWorldFrameToFixedFrameTransform(const Eigen::Isometry3d &T_W_F, const std::string &fixedFrame)
gtsam::Pose3 getInitialWorldFrameToFixedFrameTransform(const std::string &fixedFrame)
bool addFactorsToBatchSmootherAndOptimize(const gtsam::NonlinearFactorGraph &newBatchGraphFactors, const gtsam::Values &newBatchGraphValues, const std::map< gtsam::Key, double > &newBatchGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr)
gtsam::Key(* F)(std::uint64_t)
bool runSlowBatchSmootherOptimization_(int maxIterations)
bool getUnaryFactorGeneralKey(gtsam::Key &returnedKey, double &returnedGraphTime, const UnaryMeasurement &unaryMeasurement)
bool hasReferenceFrameKeyframe(const std::string &worldFrame, const std::string &fixedFrame)
std::map< double, double > updateDurationContainer_
gtsam::NavState calculateStateAtGeneralKey(bool &computeSuccessfulFlag, const gtsam::Key &generalKey)
bool logRealTimeNavStates(const std::string &savePath, const std::string &timeString)
void addUnaryHolisticFactor(const std::shared_ptr< GMSF_EXPRESSION_TYPE > gmsfUnaryExpressionPtr, const bool addToOnlineSmootherFlag=true)
std::shared_ptr< holistic_fusion::GraphConfig > graphConfigPtr_
Config.
std::map< double, double > latencyContainer_
std::shared_ptr< gtsam::NonlinearFactorGraph > batchFactorGraphBufferPtr_
bool initImuIntegrators(double gravityValue)
gtsam::Vector3 latestGraphStateKeyAngularVelocity_
static gtsam::NavState calculateNavStateAtGeneralKey(bool &computeSuccessfulFlag, std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &generalKey, const char *callingFunctionName)
std::shared_ptr< OptimizerBase > batchOptimizerPtr_
void writeValueKeysToKeyTimeStampMap_(const gtsam::Values &values, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)
std::map< double, TransformsDictionary< Eigen::Isometry3d > > realTimeReferenceFrameContainer_
bool logRealTimeReferenceFrameStates(const std::string &savePath, const std::string &timeString)
void updateImuIntegrators_(const TimeToImuMap &imuMeas)
Update IMU integrators.
void addUnaryClassicFactor(const MEASUREMENT_TYPE &unaryMeasurement, const Eigen::Matrix< double, NOISE_DIM, 1 > &unaryNoiseDensity, double measurementTime, const FACTOR_ARGS &... factorArgs)
BatchOptimizationResult extractBatchOptimizationResult_(const bool computePoseCovarianceFlag)
std::mutex operateOnGraphDataMutex_
Mutex.
std::chrono::time_point< std::chrono::high_resolution_clock > latencyEndTime_
GraphManager(std::shared_ptr< GraphConfig > graphConfigPtr, std::string imuFrame, std::string worldFrame)
std::chrono::time_point< std::chrono::high_resolution_clock > updateDurationEndTime_
Eigen::Vector3d & getInitGyrBiasReference()
bool initPoseVelocityBiasGraph(double timeStamp, const gtsam::Pose3 &T_W_I0, const gtsam::Pose3 &T_O_I0, const gtsam::Vector3 &imuAngularVelocity)
DynamicDictionaryContainer gtsamDynamicExpressionKeys_
std::shared_ptr< OptimizerBase > rtOptimizerPtr_
bool addFactorToRtAndBatchGraph_(const gtsam::NoiseModelFactor *noiseModelFactorPtr, const bool addToOnlineSmootherFlag=true)
std::chrono::time_point< std::chrono::high_resolution_clock > latencyStartTime_
bool findGraphKeys_(gtsam::Key &closestKeyKm1, gtsam::Key &closestKeyK, double &keyTimeStampDistance, double maxTimestampDistance, double timeKm1, double timeK, const std::string &name)
Find graph keys for timestamps.
std::map< double, gtsam::Pose3 > realTimeWorldPoseContainer_
holistic_fusion::GraphState optimizedGraphState_
std::shared_ptr< gtsam::NonlinearFactorGraph > rtFactorGraphBufferPtr_
Data buffer.
std::shared_ptr< std::map< gtsam::Key, double > > rtGraphKeysTimestampsMapBufferPtr_
std::shared_ptr< gtsam::Values > batchGraphValuesBufferPtr_
bool addFactorsToRtSmootherAndOptimize(const gtsam::NonlinearFactorGraph &newRtGraphFactors, const gtsam::Values &newRtGraphValues, const std::map< gtsam::Key, double > &newRtGraphKeysTimestampsMap, const std::shared_ptr< GraphConfig > &graphConfigPtr, const int additionalIterations)
gtsam::NavState W_imuPropagatedState_
Propagated state (at IMU frequency).
bool addFactorToRtAndBatchGraph_(const gtsam::NoiseModelFactor *noiseModelFactorPtr, double measurementTimestamp, const std::string &measurementName, const bool addToOnlineSmootherFlag=true)
static Eigen::Matrix< double, 6, 6 > calculatePoseCovarianceAtKeyInWorldFrame(std::shared_ptr< holistic_fusion::OptimizerBase > graphPtr, const gtsam::Key &graphKey, const char *callingFunctionName, std::optional< const gtsam::NavState > optionalNavState=std::nullopt)
gtsam::Key addPoseBetweenFactor(const gtsam::Pose3 &deltaPose, const Eigen::Matrix< double, 6, 1 > &poseBetweenNoiseDensity, double lidarTimeKm1, double lidarTimeK, double rate, const RobustNormEnum &robustNormEnum, const double robustNormConstant)
std::shared_ptr< gtsam::imuBias::ConstantBias > imuBiasPriorPtr_
std::shared_ptr< std::map< gtsam::Key, double > > batchGraphKeysTimestampsMapBufferPtr_
gtsam::Vector6 lastImuVector_
IMU Buffer.
const gtsam::Key getPropagatedStateKey()
static void saveOptimizedGraphToG2o(const OptimizerBase &optimizedGraph, const gtsam::Values &optimizedValues, const std::string &saveFileName)
bool addFactorSafelyToRtAndBatchGraph_(const gtsam::NoiseModelFactor *noiseModelFactorPtr, double measurementTimestamp)
std::chrono::time_point< std::chrono::high_resolution_clock > updateDurationStartTime_
std::shared_ptr< gtsam::Values > rtGraphValuesBufferPtr_
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements > imuBufferPreintegratorPtr_
Buffer Preintegrator.
void writeKeyToKeyTimeStampMap_(const gtsam::Key &key, double measurementTime, std::shared_ptr< std::map< gtsam::Key, double > > keyTimestampMapPtr)
Associate timestamp to each 'value key', e.g. for graph key 0, value keys (x0,v0,b0) need to be assoc...
std::shared_ptr< gtsam::PreintegratedCombinedMeasurements::Params > imuParamsPtr_
bool optimizeSlowBatchSmoother(int maxIterations, const std::string &savePath, const bool saveCovarianceFlag)
Class to represent a unary measurement in the graph.
std::map< double, holistic_fusion::ImuMeasurement, std::less< double >, Eigen::aligned_allocator< std::pair< const double, holistic_fusion::ImuMeasurement > > > TimeToImuMap
Definition ImuBuffer.hpp:35