Update visualization/logger.py

visualization/logger.py

@@ -56,14 +56,13 @@ class SimulationLogger:
 
     def log_subjects(self, subjects: List[Dict[str, Any]]) -> None:
         """
-        Updated to handle dynamic subjects:
+        Handles dynamic subjects:
         - Each subject has multiple frames (position, rotation).
         - Subject dimensions (width, height, depth) remain constant across frames.
         """
         if not subjects:
             return
 
-        # For each subject, log each frame at a different time index.
         for subject_idx, subject in enumerate(subjects):
             dimensions = subject.get("dimensions", {})
             frames = subject.get("frames", [])
@@ -80,7 +79,7 @@ class SimulationLogger:
                     # Convert Euler angles to quaternion for rotation
                     rotation_q = euler_to_quaternion(frame['rotation'])
 
-                    # 1) Log a transform for the subject so the box is placed at 'center' with correct rotation
+                    # 1) Log a transform for this subject so the box is placed at 'center' with correct rotation
                     rr.log(
                         f"world/subjects/subject_{subject_idx}",
                         rr.Transform3D(
@@ -101,19 +100,73 @@ class SimulationLogger:
                             fill_mode="solid"
                         )
                     )
+
                 except Exception as e:
                     print(f"Error creating box parameters for subject {subject_idx}, frame {frame_idx}: {str(e)}")
 
+    def log_subject_trajectories(self, subjects: List[Dict[str, Any]]) -> None:
+        """
+        For each subject, collect all frame centers and log them as:
+         - A set of 3D points (subject center positions).
+         - A line strip connecting them (showing the subject's center path).
+        This is done timelessly so the full trajectory is always visible.
+        """
+        if not subjects:
+            return
+
+        for subject_idx, subject in enumerate(subjects):
+            dimensions = subject.get("dimensions", {})
+            frames = subject.get("frames", [])
+            label = subject.get('objectClass', f"Subject_{subject_idx}")
+
+            if not frames:
+                continue
+
+            # Gather center positions
+            center_positions = []
+            for frame in frames:
+                pos = vector3_to_numpy(frame['position'])
+                center_positions.append(pos)
+
+            center_positions = np.array(center_positions, dtype=np.float32)
+
+            # Log the points
+            rr.log(
+                f"world/subjects/subject_{subject_idx}/center_trajectory_points",
+                rr.Points3D(
+                    center_positions,
+                    colors=np.full((len(center_positions), 4), [0.8, 0.6, 0.2, 1.0])
+                ),
+                timeless=True
+            )
+
+            # Log a line strip
+            if len(center_positions) > 1:
+                lines = np.stack([center_positions[:-1], center_positions[1:]], axis=1)
+                rr.log(
+                    f"world/subjects/subject_{subject_idx}/center_trajectory_line",
+                    rr.LineStrips3D(
+                        lines,
+                        colors=[(0.8, 0.6, 0.2, 1.0)],
+                    ),
+                    timeless=True
+                )
+
     def log_camera_trajectory(self, camera_frames: List[Dict[str, Any]]) -> None:
+        """
+        Logs the entire camera trajectory as a set of 3D points and a connecting line strip.
+        This is typically done timelessly to show the full path at once.
+        """
         if not camera_frames:
             return
 
         try:
             camera_positions = np.array([
                 vector3_to_numpy(frame['position']) for frame in camera_frames
-            ])
+            ], dtype=np.float32)
+
             rr.log(
-                "world/camera_trajectory",
+                "world/camera_trajectory_points",
                 rr.Points3D(
                     camera_positions,
                     colors=np.full((len(camera_positions), 4), [0.0, 0.8, 0.8, 1.0])
@@ -124,7 +177,7 @@ class SimulationLogger:
             if len(camera_positions) > 1:
                 lines = np.stack([camera_positions[:-1], camera_positions[1:]], axis=1)
                 rr.log(
-                    "world/camera_trajectory/line",
+                    "world/camera_trajectory_line",
                     rr.LineStrips3D(
                         lines,
                         colors=[(0.0, 0.8, 0.8, 1.0)]
@@ -136,6 +189,9 @@ class SimulationLogger:
             print(f"Error logging camera trajectory: {str(e)}")
 
     def log_camera_frames(self, camera_frames: List[Dict[str, Any]]) -> None:
+        """
+        Logs each camera frame at a given time sequence, so you can watch it update over time.
+        """
         if not camera_frames:
             return