HolisticFusion.h

Go to the documentation of this file.

/*
Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
All rights reserved.
This file is released under the "BSD-3-Clause License".
Please see the LICENSE file that has been included as part of this package.
 */
 
#ifndef HOLISTIC_FUSION_H
#define HOLISTIC_FUSION_H
 
// C++
#include <atomic>
#include <mutex>
#include <stdexcept>
#include <thread>
#include <unordered_set>
 
// Package
#include "holistic_fusion/config/GraphConfig.h"
#include "holistic_fusion/config/StaticTransforms.h"
#include "holistic_fusion/imu/ImuBuffer.hpp"
#include "holistic_fusion/interface/BatchOptimizationResult.h"
#include "holistic_fusion/interface/NavState.h"
#include "holistic_fusion/measurements/BinaryMeasurementXD.h"
#include "holistic_fusion/measurements/UnaryMeasurementXDAbsolute.h"
#include "holistic_fusion/measurements/UnaryMeasurementXDLandmark.h"
 
namespace holistic_fusion {
 
// Forward Declarations
class GraphManager;
 
// Actual Class
class HolisticFusion {
 public:  // Interface
  // Constructor
  HolisticFusion();
  // Destructor
  virtual ~HolisticFusion();
 
  // Setup
  void setup(const std::shared_ptr<GraphConfig> graphConfigPtr, const std::shared_ptr<StaticTransforms> staticTransformsPtr);
 
  // Initialization Interface
  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);
 
  // Trigger offline smoother optimization
  // Writes the optimized states to files in savePath. Returns false if the slow batch smoother is disabled.
  bool optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag);
  // Returns the optimized navigation states without writing files. Returns std::nullopt if the slow batch smoother is disabled.
  // Computing the pose covariances is slow for long trajectories.
  std::optional<BatchOptimizationResult> optimizeSlowBatchSmoother(int maxIterations, const bool computePoseCovarianceFlag);
 
  // Logging of the real-time states
  bool logRealTimeNavStates(const std::string& savePath);
 
  // Getter functions
  bool areYawAndPositionInited() const;
  bool areRollAndPitchInited() const;
  bool isGraphInited() const;
  bool getNormalOperationFlag() const { return normalOperationFlag_; }
 
  // Main: IMU
  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);
 
  // Pure Virtual Methods
  virtual void addUnaryPose3AbsoluteMeasurement(const UnaryMeasurementXDAbsolute<Eigen::Isometry3d, 6>& R_T_R_S,
                                                const bool addToOnlineSmootherFlag = true) = 0;
  virtual void addUnaryPosition3AbsoluteMeasurement(UnaryMeasurementXDAbsolute<Eigen::Vector3d, 3>& R_t_R_S) = 0;
  virtual void addUnaryVelocity3AbsoluteMeasurement(UnaryMeasurementXDAbsolute<Eigen::Vector3d, 3>& R_v_R_S) = 0;
  virtual void addUnaryRollAbsoluteMeasurement(const UnaryMeasurementXDAbsolute<double, 1>& roll_R_S) = 0;
  virtual void addUnaryPitchAbsoluteMeasurement(const UnaryMeasurementXDAbsolute<double, 1>& pitch_R_S) = 0;
  virtual void addUnaryYawAbsoluteMeasurement(const UnaryMeasurementXDAbsolute<double, 1>& yaw_R_S) = 0;
  virtual void addUnaryVelocity3LocalMeasurement(UnaryMeasurementXD<Eigen::Vector3d, 3>& S_v_R_S) = 0;

  virtual void addUnaryPosition3LandmarkMeasurement(UnaryMeasurementXDLandmark<Eigen::Vector3d, 3>& S_t_S_L,
                                                    const int landmarkCreationCounter) = 0;
  virtual void addUnaryBearing3LandmarkMeasurement(UnaryMeasurementXDLandmark<Eigen::Vector3d, 3>& S_bearing_S_L) = 0;

  virtual void addBinaryPose3Measurement(const BinaryMeasurementXD<Eigen::Isometry3d, 6>& delta) = 0;
 
  // Ambiguous Measurements
  bool addZeroMotionFactor(double timeKm1, double timeK, double noiseDensity);
  bool addZeroVelocityFactor(double timeK, double noiseDensity);
 
 protected:
  // Methods -------------
  template <int DIM>
  bool isCovarianceViolated_(const Eigen::Matrix<double, DIM, 1>& covariance, const double covarianceViolationThreshold) {
    if (covarianceViolationThreshold > 0.0) {
      for (int i = 0; i < DIM; i++) {
        if (covariance(i) > covarianceViolationThreshold) {
          return true;
        }
      }
    }
    return false;
  }
 
  // Initialization
  void pretendFirstMeasurementReceived();
 
  // Check whether measurement violated covariance, if yes, add to set and print once, if not, remove from set and print that returned
  bool checkAndPrintCovarianceViolation_(const std::string& measurementName, const bool violatedFlag);
 
  // Members
  // Graph Config
  std::shared_ptr<GraphConfig> graphConfigPtr_ = nullptr;
  // Extrinsics
  std::shared_ptr<StaticTransforms> staticTransformsPtr_ = nullptr;
 
  // Factor graph
  std::shared_ptr<GraphManager> graphMgrPtr_ = nullptr;
  // Imu Buffer
  std::shared_ptr<holistic_fusion::ImuBuffer> coreImuBufferPtr_;
 
  // Mutex
  std::mutex optimizeGraphMutex_;

  bool alignedImuFlag_ = false;
  bool foundInitialYawAndPositionFlag_ = false;
  bool initedGraphFlag_ = false;
  bool validFirstMeasurementReceivedFlag_ = false;
  bool optimizeGraphFlag_ = false;
  bool normalOperationFlag_ = false;
 
  // Counter
  long imuCallbackCounter_ = 0;
 
  // Set of measurements that have violated the covariance
  std::unordered_set<std::string> measurementsWithViolatedCovariance_;
 
 private:
  bool alignImu_(double& imuAttitudeRoll, double& imuAttitudePitch);
  void initGraph_(const double timeStamp_k, const Eigen::Vector3d& imuAngularVelocity);
  void optimizeGraph_();

  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);
 
  // Threads
  std::thread optimizeGraphThread_;  
  std::atomic<bool> stopOptimizeGraphThread_{false};
 
  // Mutex
  std::mutex initYawAndPositionMutex_;

  std::shared_ptr<SafeIntegratedNavState> preIntegratedNavStatePtr_ = nullptr;
};
 
}  // namespace holistic_fusion
 
#endif  // HOLISTIC_FUSION_H