logging of real time estimates of reference frame transformations

nubertj · nubertj · commit 99bba360782b · 2025-01-27T02:31:03.000+01:00
diff --git a/examples/anymal_estimator_graph/config/core/core_graph_config.yaml b/examples/anymal_estimator_graph/config/core/core_graph_config.yaml
@@ -2,4 +2,4 @@
 common_params:
   verbosity: 0               #Debug Output, 0: only important events, 1: functioning and state machine, 2: optimization effort, 3: added factors
   odomNotJumpAtStart: true   #Whether the World->Odom transform should be updated in the beginning (odom start at identity)
-  logRealTimeStateToMemory: true # Whether the real-time state should be logged to memory (for logging purposes)
+  logRealTimeStateToMemory: true  # Whether the real-time state should be logged to memory (for logging purposes)
diff --git a/examples/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml b/examples/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml
@@ -1,9 +1,9 @@
 # Sensor Params
 sensor_params:
-  imuRate: 400                            #Rate of IMU input (Hz)
-  createStateEveryNthImuMeasurement: 10    # Create a new state every n-th IMU measurement
-  useImuSignalLowPassFilter: false        #If true, a low pass filter is applied to the IMU measurements
-  imuLowPassFilterCutoffFreq: 30.0        #Cutoff frequency of the low pass filter
+  imuRate: 400                            # Rate of IMU input (Hz)
+  createStateEveryNthImuMeasurement: 10   # Create a new state every n-th IMU measurement
+  useImuSignalLowPassFilter: false        # If true, a low pass filter is applied to the IMU measurements
+  imuLowPassFilterCutoffFreq: 30.0        # Cutoff frequency of the low pass filter
   imuBufferLength: 800
   imuTimeOffset: 0.0                      # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot
 
diff --git a/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h b/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h
@@ -109,7 +109,6 @@ class AnymalEstimator : public graph_msf::GraphMsfRos {
   Eigen::Matrix<double, 6, 1> lioSe3AlignmentRandomWalk_ = 0.0 * Eigen::Matrix<double, 6, 1>::Ones();
 
   // Noise
-  double gnssPositionUnaryNoise_ = 1.0;  // in [m]
   Eigen::Matrix<double, 6, 1> lioPoseUnaryNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones();
   Eigen::Matrix<double, 6, 1> lioPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones();
   Eigen::Matrix<double, 6, 1> legPoseBetweenNoise_ = 1.0 * Eigen::Matrix<double, 6, 1>::Ones();
diff --git a/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch b/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch
@@ -12,10 +12,10 @@
 
     <!-- Input Topics -->
     <arg name="imu_topic_name" default="/sensors/imu"/>
-    <!--    <arg name="lidar_odometry_topic_name"-->
-    <!--         default="/open3d_slam/scan2map_odometry"/>  &lt;!&ndash;/compslam_lio/odom_aft_mapped_to_init_CORRECTED  &ndash;&gt;-->
     <arg name="lidar_odometry_topic_name"
-         default="/open3d_slam/optimized_poses_as_odometry"/>
+         default="/open3d_slam/scan2map_odometry"/>  <!--/compslam_lio/odom_aft_mapped_to_init_CORRECTED  -->
+    <!--    <arg name="lidar_odometry_topic_name"-->
+    <!--         default="/open3d_slam/optimized_poses_as_odometry"/>-->
 
 
     <!-- Load dumped parameters, mainly for description -->
diff --git a/examples/anymal_estimator_graph/src/lib/readParams.cpp b/examples/anymal_estimator_graph/src/lib/readParams.cpp
@@ -50,9 +50,6 @@ void AnymalEstimator::readParams(const ros::NodeHandle& privateNode) {
       lioSe3AlignmentRandomWalk[3], lioSe3AlignmentRandomWalk[4], lioSe3AlignmentRandomWalk[5];
 
   // Noise Parameters ---------------------------------------------------
-  /// Gnss
-  gnssPositionUnaryNoise_ = graph_msf::tryGetParam<double>("noise_params/gnssPositionUnaryNoiseDensity", privateNode);
-
   /// LiDAR Odometry
   const auto poseUnaryNoise =
       graph_msf::tryGetParam<std::vector<double>>("noise_params/lioPoseUnaryNoiseDensity", privateNode);  // roll,pitch,yaw,x,y,z
diff --git a/graph_msf/include/graph_msf/core/GraphManager.h b/graph_msf/include/graph_msf/core/GraphManager.h
@@ -92,8 +92,11 @@ class GraphManager {
   // Slow Graph Update (if desired)
   bool optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag);
 
-  // Logging of real-time states
-  bool logRealTimeStates(const std::string& savePath, const std::string& timeString);
+  // Logging of real-time navigation states
+  bool logRealTimeNavStates(const std::string& savePath, const std::string& timeString);
+
+  // Logging of real-time reference frame states
+  bool logRealTimeReferenceFrameStates(const std::string& savePath, const std::string& timeString);
 
   // Save Variables to File
   void saveOptimizedValuesToFile(const gtsam::Values& optimizedValues, const std::map<gtsam::Key, double>& keyTimestampMap,
@@ -211,6 +214,9 @@ class GraphManager {
   // Real-time Pose Container
   std::map<double, gtsam::Pose3> realTimeWorldPoseContainer_ = {};
   std::map<double, gtsam::Pose3> realTimeOdomPoseContainer_ = {};
+  // Real-time Reference Frame Transformation Container
+  std::map<double, TransformsDictionary<Eigen::Isometry3d>> realTimeReferenceFrameContainer_ = {};
+  std::set<std::pair<std::string, std::string>> realTimeReferenceFrameNamePairs_ = {};
 
   // Member variables
   /// Mutex
diff --git a/graph_msf/include/graph_msf/core/TransformsDictionary.h b/graph_msf/include/graph_msf/core/TransformsDictionary.h
@@ -23,6 +23,13 @@ class TransformsDictionary {
   // Constructor
   TransformsDictionary(TRANSFORM_TYPE identityObject) : identity_(identityObject) {}
 
+  // Copy Constructor
+  TransformsDictionary(const TransformsDictionary& other) {
+    T_frame1_frame2_map_ = other.T_frame1_frame2_map_;
+    identity_ = other.identity_;
+    numStoredTransforms_ = other.numStoredTransforms_;
+  }
+
   // Getters ------------------------------------------------------------
   // Check for specific transformation pair
   bool isFramePairInDictionary(const std::string& frame1, const std::string& frame2) {
diff --git a/graph_msf/include/graph_msf/interface/GraphMsf.h b/graph_msf/include/graph_msf/interface/GraphMsf.h
@@ -48,7 +48,7 @@ class GraphMsf {
   bool optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag);
 
   // Logging of the real-time states
-  bool logRealTimeStates(const std::string& savePath);
+  bool logRealTimeNavStates(const std::string& savePath);
 
   // Getter functions
   bool areYawAndPositionInited() const;
diff --git a/graph_msf/src/lib/GraphManager.cpp b/graph_msf/src/lib/GraphManager.cpp
@@ -533,7 +533,7 @@ void GraphManager::updateGraph() {
   gtsam::Matrix33 resultVelocityCovariance =
       rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsam::symbol_shorthand::V(currentPropagatedKey));
 
-  // D. FixedFrame Transformations ------------------------------
+  // D. Reference Frame Transformations ------------------------------
   // Containers
   TransformsDictionary<Eigen::Isometry3d> resultReferenceFrameTransformations(Eigen::Isometry3d::Identity());
   TransformsDictionary<Eigen::Matrix<double, 6, 6>> resultReferenceFrameTransformationsCovariance(gtsam::Z_6x6);
@@ -547,6 +547,9 @@ void GraphManager::updateGraph() {
 
     // Iterate through all dynamically allocated variables (holistic, calibration, landmarks) --------------------------------
     for (auto& framePairKeyMapIterator : gtsamDynamicExpressionKeys_.get<gtsam::Pose3>().getTransformsMap()) {
+      // Frame Pair
+      const std::pair<std::string, std::string>& framePair = framePairKeyMapIterator.first;  // alias
+
       // Get Variable Key
       const gtsam::Key& gtsamKey = framePairKeyMapIterator.second.key();  // alias
 
@@ -606,17 +609,30 @@ void GraphManager::updateGraph() {
                            T_frame2_frame1_corrected.translation() + framePairKeyMapIterator.second.getReferenceFrameKeyframePosition());
           gtsam::Pose3 T_frame1_frame2_corrected = T_frame2_frame1_corrected.inverse();
           // Write to container
+          // 1: For Landmark States
           if (isLandmarkState) {
+            // Write to landmark container
             resultLandmarkTransformations.set_T_frame1_frame2(framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second,
                                                               Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
             resultLandmarkTransformationsCovariance.set_T_frame1_frame2(framePairKeyMapIterator.first.first,
                                                                         framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
-          } else {
+          }
+          // 2: For Reference Frame States
+          else {
+            // Write to reference frame container
             resultReferenceFrameTransformations.set_T_frame1_frame2(framePairKeyMapIterator.first.first,
                                                                     framePairKeyMapIterator.first.second,
                                                                     Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
             resultReferenceFrameTransformationsCovariance.set_T_frame1_frame2(
                 framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
+            // Potentially fill in existing pairs for logging
+            if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
+              if (realTimeReferenceFrameNamePairs_.find(framePair) == realTimeReferenceFrameNamePairs_.end()) {
+                realTimeReferenceFrameNamePairs_.insert(framePair);
+                REGULAR_COUT << " Added frame pair: " << framePair.first << " to " << framePair.second << " to real-time logging."
+                             << std::endl;
+              }
+            }
           }
 
           // Mark that this key has at least been optimized once
@@ -697,8 +713,8 @@ void GraphManager::updateGraph() {
             }
           }
         }  // catch statement
-      }    // end: if active statement
-    }      // for loop over all transforms
+      }  // end: if active statement
+    }  // for loop over all transforms
   }
 
   // Mutex block 2 ------------------
@@ -740,6 +756,11 @@ void GraphManager::updateGraph() {
     // Update the time of the last optimized state
     lastOptimizedStateTime_ = currentPropagatedTime;
   }  // end of locking
+
+  // Potentially log real-time state to container
+  if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
+    realTimeReferenceFrameContainer_.emplace(currentPropagatedTime, resultReferenceFrameTransformations);
+  }
 }
 
 bool GraphManager::optimizeSlowBatchSmoother(int maxIterations, const std::string& savePath, const bool saveCovarianceFlag) {
@@ -768,16 +789,16 @@ bool GraphManager::optimizeSlowBatchSmoother(int maxIterations, const std::strin
   }
 }
 
-// Logging of the real-time states
-bool GraphManager::logRealTimeStates(const std::string& savePath, const std::string& timeString) {
+// Logging of the real-time navigation states
+bool GraphManager::logRealTimeNavStates(const std::string& savePath, const std::string& timeString) {
   // Note enabled
   if (!graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
     REGULAR_COUT << RED_START << " Logging of real-time states is disabled. " << COLOR_END << std::endl;
     return false;
   }
   // Enabled
   else {
-    // Main container: map, although only one element
+    // Main container: map
     std::map<std::string, std::ofstream> fileStreams;
 
     // Create directory if it does not exist
@@ -814,6 +835,62 @@ bool GraphManager::logRealTimeStates(const std::string& savePath, const std::str
   }
 }
 
+// Logging of real-time reference frame states
+bool GraphManager::logRealTimeReferenceFrameStates(const std::string& savePath, const std::string& timeString) {
+  // Note enabled
+  if (!graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
+    REGULAR_COUT << RED_START << " Logging of real-time states is disabled. " << COLOR_END << std::endl;
+    return false;
+  }
+  // Enabled
+  else {
+    // Main container: map
+    std::map<std::string, std::ofstream> fileStreams;
+
+    // Create directory if it does not exist
+    if (!std::filesystem::exists(savePath + timeString)) {
+      std::filesystem::create_directories(savePath + timeString);
+    }
+
+    // State Category String
+    std::string stateCategoryString = "rt_R_6D_transform";
+
+    // Go through all reference frame states that have been logged
+    for (const std::pair<std::string, std::string>& framePair : realTimeReferenceFrameNamePairs_) {
+      // Frame Information
+      std::string frameInformation = "_" + framePair.first + "_to_" + framePair.second;
+      // Create File Streams
+      // CSV
+      FileLogger::createPose3CsvFileStream(fileStreams, savePath, stateCategoryString + frameInformation, timeString, false);
+      // TUM
+      FileLogger::createPose3TumFileStream(fileStreams, savePath, stateCategoryString + frameInformation + "_tum", timeString);
+    }
+
+    // Iterate through all timestamps and corresponding transform maps
+    for (const auto& mapPair : realTimeReferenceFrameContainer_) {
+      const double& timeStamp = mapPair.first;  // alias
+      const TransformsDictionary<Eigen::Isometry3d>& referenceFrameTransforms = mapPair.second;
+      // Iterate through all transforms for this time stamp
+      for (const auto& framePairTransform : referenceFrameTransforms.getTransformsMap()) {
+        // Frame Pair
+        const std::pair<std::string, std::string>& framePair = framePairTransform.first;  // alias
+        // Transform
+        const Eigen::Isometry3d& transform = framePairTransform.second;  // alias
+        // Frame Information
+        std::string frameInformation = "_" + framePair.first + "_to_" + framePair.second;
+        // Write to files
+        // CSV
+        FileLogger::writePose3ToCsvFile(fileStreams, gtsam::Pose3(transform.matrix()), stateCategoryString + frameInformation, timeStamp,
+                                        false);
+        // TUM
+        FileLogger::writePose3ToTumFile(fileStreams, gtsam::Pose3(transform.matrix()), stateCategoryString + frameInformation + "_tum",
+                                        timeStamp);
+      }
+    }
+    return true;
+  }
+}
+
 // Save optimized values to file
 void GraphManager::saveOptimizedValuesToFile(const gtsam::Values& optimizedValues, const std::map<gtsam::Key, double>& keyTimestampMap,
                                              const std::string& savePath, const bool saveCovarianceFlag) {
@@ -846,7 +923,10 @@ void GraphManager::saveOptimizedValuesToFile(const gtsam::Values& optimizedValue
 
   // Save real-time states
   if (graphConfigPtr_->logRealTimeStateToMemoryFlag_) {
-    std::ignore = logRealTimeStates(savePath, timeString);
+    // Real-time states
+    std::ignore = logRealTimeNavStates(savePath, timeString);
+    // Real-time Reference Frame States
+    std::ignore = logRealTimeReferenceFrameStates(savePath, timeString);
   }
 
   // Save optimized states
diff --git a/graph_msf/src/lib/GraphMsf.cpp b/graph_msf/src/lib/GraphMsf.cpp
@@ -54,7 +54,7 @@ bool GraphMsf::optimizeSlowBatchSmoother(int maxIterations, const std::string& s
   return graphMgrPtr_->optimizeSlowBatchSmoother(maxIterations, savePath, saveCovarianceFlag);
 }
 
-bool GraphMsf::logRealTimeStates(const std::string& savePath) {
+bool GraphMsf::logRealTimeNavStates(const std::string& savePath) {
   // String of time without line breaks: year_month_day_hour_min_sec
   std::ostringstream oss;
   std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
@@ -63,7 +63,7 @@ bool GraphMsf::logRealTimeStates(const std::string& savePath) {
   // Convert stream to string
   std::string timeString = oss.str();
   // Return
-  return graphMgrPtr_->logRealTimeStates(savePath, timeString);
+  return graphMgrPtr_->logRealTimeNavStates(savePath, timeString);
 }
 
 // Getter functions -----------------------
diff --git a/ros/graph_msf_ros/include/graph_msf_ros/GraphMsfRos.h b/ros/graph_msf_ros/include/graph_msf_ros/GraphMsfRos.h
@@ -88,7 +88,7 @@ class GraphMsfRos : public GraphMsfClassic, public GraphMsfHolistic {
   // Services
   virtual bool srvOfflineSmootherOptimizeCallback(graph_msf_ros_msgs::OfflineOptimizationTrigger::Request& req,
                                                   graph_msf_ros_msgs::OfflineOptimizationTrigger::Response& res);
-  bool srvLogRealTimeStatesCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+  bool srvLogRealTimeNavStatesCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
 
   // Publishing -----------------------------------
   // Higher Level Functions
@@ -191,7 +191,7 @@ class GraphMsfRos : public GraphMsfClassic, public GraphMsfHolistic {
   // Trigger offline smoother optimization
   ros::ServiceServer srvSmootherOptimize_;
   // Log Real-Time States
-  ros::ServiceServer srvLogRealTimeStates_;
+  ros::ServiceServer srvLogRealTimeNavStates_;
 
   // Last Optimized State Timestamp
   double lastOptimizedStateTimestamp_ = 0.0;
diff --git a/ros/graph_msf_ros/src/lib/GraphMsfRos.cpp b/ros/graph_msf_ros/src/lib/GraphMsfRos.cpp
@@ -117,7 +117,7 @@ void GraphMsfRos::initializeServices(ros::NodeHandle& privateNode) {
   srvSmootherOptimize_ =
       privateNode.advertiseService("/graph_msf/trigger_offline_optimization", &GraphMsfRos::srvOfflineSmootherOptimizeCallback, this);
   // Real-Time State Logging
-  srvLogRealTimeStates_ = privateNode.advertiseService("/graph_msf/log_real_time_states", &GraphMsfRos::srvLogRealTimeStatesCallback, this);
+  srvLogRealTimeNavStates_ = privateNode.advertiseService("/graph_msf/log_real_time_nav_states", &GraphMsfRos::srvLogRealTimeNavStatesCallback, this);
 }
 
 bool GraphMsfRos::srvOfflineSmootherOptimizeCallback(graph_msf_ros_msgs::OfflineOptimizationTrigger::Request& req,
@@ -137,9 +137,9 @@ bool GraphMsfRos::srvOfflineSmootherOptimizeCallback(graph_msf_ros_msgs::Offline
   return true;
 }
 
-bool GraphMsfRos::srvLogRealTimeStatesCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+bool GraphMsfRos::srvLogRealTimeNavStatesCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
   // Log real-time states
-  if (GraphMsf::logRealTimeStates(optimizationResultLoggingPath)) {
+  if (GraphMsf::logRealTimeNavStates(optimizationResultLoggingPath)) {
     res.success = true;
     res.message = "Logging real-time states.";
   } else {
@@ -269,7 +269,7 @@ void GraphMsfRos::createVelocityMarker(const std::string& referenceFrameName, co
   startPoint = geometry_msgs::Point();
   startPoint.x = 0.0;  // origin
   startPoint.y = 0.0;  // origin
-  startPoint.z = 1.0;  // 1 meter above origin
+  startPoint.z = 0.0;  // origin
   endPoint = geometry_msgs::Point();
   endPoint.x = startPoint.x + velocity(0);
   endPoint.y = startPoint.y + velocity(1);
