chore: updates tutorial docs

Signed-off-by: vividf <[email protected]>

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp

@@ -60,8 +60,7 @@ class Visualization
 
   visualization_msgs::msg::MarkerArray deleteTrackMarkers(const size_t converged_tracks_size);
   visualization_msgs::msg::Marker drawCalibrationStatusText(
-    const size_t converged_tracks_size,
-    std::unordered_map<TransformationType, CalibrationErrorMetrics> methods);
+    const size_t converged_tracks_size, TransformationType type, CalibrationErrorMetrics metrics);
 
 private:
   VisualizationParamters params_;

calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py

@@ -21,9 +21,7 @@
 import numpy as np
 import rclpy
 from rclpy.node import Node
-from tier4_calibration_msgs.msg import (
-    CalibrationMetrics,  # Adjust the import for the new message type
-)
+from tier4_calibration_msgs.msg import CalibrationMetrics
 
 
 class MetricsPlotter:
@@ -156,36 +154,22 @@ def update_metrics(self, msg):
             metrics["std_crossval_yaw_error_list"] = method.std_crossval_calibrated_yaw_error
 
     def compute_detection_metrics(self, detections):
+        epsilon = 1e-6
         ranges = []
         pitches = []
         yaws = []
 
         for detection in detections:
-            # Print detection coordinates for debugging
-            print(f"Detection point: x={detection.x}, y={detection.y}, z={detection.z}")
-
-            # Compute range
             range_ = math.sqrt(detection.x**2 + detection.y**2 + detection.z**2)
-            if range_ == 0:
-                print("Skipping detection with zero range.")
-                continue  # Skip invalid detections
+            range_ = max(range_, epsilon)
+            ranges.append(range_)
 
-            # Compute pitch angle (clamped to valid range)
             pitch = math.degrees(math.asin(max(-1.0, min(1.0, detection.z / range_))))
             pitches.append(pitch)
 
-            # Compute yaw angle
             yaw = math.degrees(math.atan2(detection.y, detection.x))
             yaws.append(yaw)
 
-            # Add range
-            ranges.append(range_)
-
-        # Print computed values for debugging
-        print(f"Ranges: {ranges}")
-        print(f"Pitches: {pitches}")
-        print(f"Yaws: {yaws}")
-
         return ranges, pitches, yaws
 
     def plot_detection_distributions(self, ranges, pitches, yaws):
@@ -266,82 +250,61 @@ def draw_subplots(self):
             subplots["average_distance"].clear()
             subplots["average_yaw"].clear()
 
-            # Draw cross-validation plots
-            if metrics["crossval_sample_list"] and metrics["crossval_distance_error_list"]:
-                subplots["crossval_distance"].plot(
-                    metrics["crossval_sample_list"],
-                    metrics["crossval_distance_error_list"],
-                    self.color_distance_o,
-                )
-            if metrics["crossval_sample_list"] and metrics["crossval_yaw_error_list"]:
-                subplots["crossval_yaw"].plot(
-                    metrics["crossval_sample_list"],
-                    metrics["crossval_yaw_error_list"],
-                    self.color_yaw_o,
-                )
+            # Filter data where the number of reflectors >= 3
+            filtered_indices = [
+                i for i, num in enumerate(metrics["num_of_reflectors_list"]) if num >= 3
+            ]
 
-            # Draw average error plots
-            if metrics["num_of_reflectors_list"] and metrics["calibration_distance_error_list"]:
+            filtered_reflectors = [metrics["num_of_reflectors_list"][i] for i in filtered_indices]
+            filtered_distance_errors = [
+                metrics["calibration_distance_error_list"][i] for i in filtered_indices
+            ]
+            filtered_yaw_errors = [
+                metrics["calibration_yaw_error_list"][i] for i in filtered_indices
+            ]
+
+            if filtered_reflectors and filtered_distance_errors:
                 subplots["average_distance"].plot(
-                    metrics["num_of_reflectors_list"],
-                    metrics["calibration_distance_error_list"],
+                    filtered_reflectors,
+                    filtered_distance_errors,
                     self.color_distance_o,
                 )
-            if metrics["num_of_reflectors_list"] and metrics["calibration_yaw_error_list"]:
+                subplots["average_distance"].annotate(
+                    f"{filtered_distance_errors[-1]:.2f}",
+                    xy=(filtered_reflectors[-1], filtered_distance_errors[-1]),
+                    color=self.color_distance,
+                )
+
+            if filtered_reflectors and filtered_yaw_errors:
                 subplots["average_yaw"].plot(
-                    metrics["num_of_reflectors_list"],
-                    metrics["calibration_yaw_error_list"],
+                    filtered_reflectors,
+                    filtered_yaw_errors,
                     self.color_yaw_o,
                 )
+                subplots["average_yaw"].annotate(
+                    f"{filtered_yaw_errors[-1]:.2f}",
+                    xy=(filtered_reflectors[-1], filtered_yaw_errors[-1]),
+                    color=self.color_yaw,
+                )
 
-            # Annotate the last value for average distance/yaw
-            if metrics["num_of_reflectors_list"]:
-                if metrics["calibration_distance_error_list"]:
-                    subplots["average_distance"].annotate(
-                        f"{metrics['calibration_distance_error_list'][-1]:.2f}",
-                        xy=(
-                            metrics["num_of_reflectors_list"][-1],
-                            metrics["calibration_distance_error_list"][-1],
-                        ),
-                        color=self.color_distance,
-                    )
-                if metrics["calibration_yaw_error_list"]:
-                    subplots["average_yaw"].annotate(
-                        f"{metrics['calibration_yaw_error_list'][-1]:.2f}",
-                        xy=(
-                            metrics["num_of_reflectors_list"][-1],
-                            metrics["calibration_yaw_error_list"][-1],
-                        ),
-                        color=self.color_yaw,
-                    )
-
-            # Annotate the last value for cross-validation distance/yaw
-            if metrics["crossval_sample_list"]:
-                if metrics["crossval_distance_error_list"]:
-                    subplots["crossval_distance"].annotate(
-                        f"{metrics['crossval_distance_error_list'][-1]:.2f}",
-                        xy=(
-                            metrics["crossval_sample_list"][-1],
-                            metrics["crossval_distance_error_list"][-1],
-                        ),
-                        color=self.color_distance,
-                    )
-                if metrics["crossval_yaw_error_list"]:
-                    subplots["crossval_yaw"].annotate(
-                        f"{metrics['crossval_yaw_error_list'][-1]:.2f}",
-                        xy=(
-                            metrics["crossval_sample_list"][-1],
-                            metrics["crossval_yaw_error_list"][-1],
-                        ),
-                        color=self.color_yaw,
-                    )
-
-            # Draw standard deviation bands for cross-validation
             if (
                 metrics["crossval_sample_list"]
                 and metrics["crossval_distance_error_list"]
                 and metrics["std_crossval_distance_error_list"]
             ):
+                subplots["crossval_distance"].plot(
+                    metrics["crossval_sample_list"],
+                    metrics["crossval_distance_error_list"],
+                    self.color_distance_o,
+                )
+                subplots["crossval_distance"].annotate(
+                    f"{metrics['crossval_distance_error_list'][-1]:.2f}",
+                    xy=(
+                        metrics["crossval_sample_list"][-1],
+                        metrics["crossval_distance_error_list"][-1],
+                    ),
+                    color=self.color_distance,
+                )
                 subplots["crossval_distance"].fill_between(
                     metrics["crossval_sample_list"],
                     np.array(metrics["crossval_distance_error_list"])
@@ -351,11 +314,25 @@ def draw_subplots(self):
                     color=self.color_distance,
                     alpha=0.3,
                 )
+
             if (
                 metrics["crossval_sample_list"]
                 and metrics["crossval_yaw_error_list"]
                 and metrics["std_crossval_yaw_error_list"]
             ):
+                subplots["crossval_yaw"].plot(
+                    metrics["crossval_sample_list"],
+                    metrics["crossval_yaw_error_list"],
+                    self.color_yaw_o,
+                )
+                subplots["crossval_yaw"].annotate(
+                    f"{metrics['crossval_yaw_error_list'][-1]:.2f}",
+                    xy=(
+                        metrics["crossval_sample_list"][-1],
+                        metrics["crossval_yaw_error_list"][-1],
+                    ),
+                    color=self.color_yaw,
+                )
                 subplots["crossval_yaw"].fill_between(
                     metrics["crossval_sample_list"],
                     np.array(metrics["crossval_yaw_error_list"])

calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp

@@ -471,8 +471,9 @@ void ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback(
     auto tracking_markers =
       visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_);
     tracking_markers_pub_->publish(tracking_markers);
-    auto text_markers =
-      visualization_.drawCalibrationStatusText(converged_tracks_.size(), output_metrics_.methods);
+    auto text_markers = visualization_.drawCalibrationStatusText(
+      converged_tracks_.size(), transformation_type_,
+      output_metrics_.methods[transformation_type_]);
     text_markers_pub_->publish(text_markers);
 
     response->success = true;
@@ -538,8 +539,8 @@ void ExtrinsicReflectorBasedCalibrator::loadDatabaseCallback(
   auto tracking_markers =
     visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_);
   tracking_markers_pub_->publish(tracking_markers);
-  auto text_markers =
-    visualization_.drawCalibrationStatusText(converged_tracks_.size(), output_metrics_.methods);
+  auto text_markers = visualization_.drawCalibrationStatusText(
+    converged_tracks_.size(), transformation_type_, output_metrics_.methods[transformation_type_]);
   text_markers_pub_->publish(text_markers);
 }
 
@@ -646,8 +647,8 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback(
   auto tracking_markers =
     visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_);
   tracking_markers_pub_->publish(tracking_markers);
-  auto text_markers =
-    visualization_.drawCalibrationStatusText(converged_tracks_.size(), output_metrics_.methods);
+  auto text_markers = visualization_.drawCalibrationStatusText(
+    converged_tracks_.size(), transformation_type_, output_metrics_.methods[transformation_type_]);
   text_markers_pub_->publish(text_markers);
 
   RCLCPP_INFO(
@@ -1599,15 +1600,6 @@ TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation()
     transformation_result
       .calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d] =
       estimator.getTransformation();
-    RCLCPP_INFO_STREAM(
-      this->get_logger(), "Initial radar->lidar transform:\n"
-                            << initial_radar_to_lidar_eigen_.matrix());
-    RCLCPP_INFO_STREAM(
-      this->get_logger(),
-      "Pure rotation calibration radar->lidar transform:\n"
-        << transformation_result
-             .calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d]
-             .matrix());
 
     // svd 2d transformation
     std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) =
@@ -1617,16 +1609,16 @@ TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation()
     estimator.estimateSVDTransformation(transformation_type_);
     transformation_result.calibrated_radar_to_lidar_transformations[TransformationType::svd_2d] =
       estimator.getTransformation();
+
     RCLCPP_INFO_STREAM(
       this->get_logger(), "Initial radar->lidar transform:\n"
                             << initial_radar_to_lidar_eigen_.matrix());
     RCLCPP_INFO_STREAM(
       this->get_logger(),
-      "Yaw only 2D calibration radar->lidar transform:\n"
+      "Yaw-only roatation 2D radar->lidar transform:\n"
         << transformation_result
-             .calibrated_radar_to_lidar_transformations[TransformationType::svd_2d]
+             .calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d]
              .matrix());
-
     RCLCPP_INFO_STREAM(
       this->get_logger(),
       "SVD 2D calibration radar->lidar transform:\n"

calibrators/marker_radar_lidar_calibrator/src/visualization.cpp

@@ -17,7 +17,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <string>
-#include <unordered_map>
 #include <vector>
 
 namespace marker_radar_lidar_calibrator
@@ -249,8 +248,7 @@ visualization_msgs::msg::MarkerArray Visualization::deleteTrackMarkers(
 }
 
 visualization_msgs::msg::Marker Visualization::drawCalibrationStatusText(
-  const size_t converged_tracks_size,
-  std::unordered_map<TransformationType, CalibrationErrorMetrics> methods)
+  const size_t converged_tracks_size, TransformationType type, CalibrationErrorMetrics metrics)
 {
   visualization_msgs::msg::Marker text_marker;
 
@@ -264,31 +262,21 @@ visualization_msgs::msg::Marker Visualization::drawCalibrationStatusText(
   text_marker.ns = "calibration_status";
   text_marker.scale.z = 0.3;
 
-  // show the latest cross validation results which is located in the last two elements of the
-  // metrics vector show the latest calibration result, which is located in the 2nd and 3rd index of
-  // the metrics vector
-
-  if (!converged_tracks_size) {
-    text_marker.text = " pairs=" + std::to_string(converged_tracks_size);
-  } else {
-    text_marker.text = " pairs=" + std::to_string(converged_tracks_size);
-
-    for (const auto & [type, metrics] : methods) {
-      // Display average errors
-      text_marker.text += "\n " + toString(type) + ": average_distance_error[cm]=" +
-                          toStringWithPrecision(metrics.calibrated_distance_error * m_to_cm, 2) +
-                          "\n " + toString(type) + ": average_yaw_error[deg]=" +
-                          toStringWithPrecision(metrics.calibrated_yaw_error, 2);
-
-      // Display cross-validation errors
-      if (converged_tracks_size > 3) {
-        text_marker.text +=
-          "\n " + toString(type) + ": crossval_distance_error[cm]=" +
-          toStringWithPrecision(
-            metrics.avg_crossval_calibrated_distance_error.back() * m_to_cm, 2) +
-          "\n " + toString(type) + ": crossval_yaw_error[deg]=" +
-          toStringWithPrecision(metrics.avg_crossval_calibrated_yaw_error.back(), 2);
-      }
+  text_marker.text = toString(type) + "\npairs=" + std::to_string(converged_tracks_size);
+  if (converged_tracks_size) {
+    // Display average errors
+    text_marker.text +=
+      "\naverage_distance_error[cm]=" +
+      toStringWithPrecision(metrics.calibrated_distance_error * m_to_cm, 2) +
+      "\naverage_yaw_error[deg]=" + toStringWithPrecision(metrics.calibrated_yaw_error, 2);
+
+    // Display cross-validation errors
+    if (converged_tracks_size > 3) {
+      text_marker.text +=
+        "\ncrossval_distance_error[cm]=" +
+        toStringWithPrecision(metrics.avg_crossval_calibrated_distance_error.back() * m_to_cm, 2) +
+        "\ncrossval_yaw_error[deg]=" +
+        toStringWithPrecision(metrics.avg_crossval_calibrated_yaw_error.back(), 2);
     }
   }
   text_marker.pose.position.x = 1.0;