Minor virtual radar improvements

.gitignore

@@ -14,3 +14,4 @@ src/vtr3_pyposegraph.egg-info
 src/vtr_gps_plotting
 *.db3
 build
+point_clouds/

helpers/pix4d2radar.py

@@ -0,0 +1,55 @@
+import open3d as o3d
+import numpy as np
+
+# Input and output file paths
+input_ply = "point_clouds/pin_4_crop_sub.ply"
+output_ply = "point_clouds/pin_4_filtered.ply"
+
+# Read the point cloud
+pcd = o3d.io.read_point_cloud(input_ply)
+
+# Convert to numpy array
+points = np.asarray(pcd.points)
+
+pix4d_pc_base_height = -12
+radar_height = 1.5
+radar_angle = 2  # degrees
+radar_range = 20  # meters
+
+# Calculate centroid of x and y coordinates
+centroid_x = np.mean(points[:, 0])
+centroid_y = np.mean(points[:, 1])
+
+# Radar position in the point cloud coordinate system
+# Using centroid of points for x and y coordinates
+radar_position = np.array([centroid_x, centroid_y, pix4d_pc_base_height + radar_height])
+
+# Calculate horizontal distance of each point from radar in x-y plane
+distances = np.sqrt((points[:, 0] - radar_position[0])**2 + 
+                    (points[:, 1] - radar_position[1])**2)
+
+# Calculate allowed z-range at each distance based on radar_angle
+allowed_deviation = distances * np.tan(np.deg2rad(radar_angle))
+
+# Calculate upper and lower bounds for each point
+lower_bounds = radar_position[2] - allowed_deviation
+upper_bounds = radar_position[2] + allowed_deviation
+
+# Filter points that fall within the conic field of view
+mask = (points[:, 2] >= lower_bounds) & (points[:, 2] <= upper_bounds) & (distances <= radar_range)
+filtered_points = points[mask]
+
+# Set z coordinates to 0
+filtered_points[:, 2] = 0
+
+# Create new point cloud
+filtered_pcd = o3d.geometry.PointCloud()
+filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
+
+# Optionally, copy colors if present
+if pcd.has_colors():
+    colors = np.asarray(pcd.colors)
+    filtered_pcd.colors = o3d.utility.Vector3dVector(colors[mask])
+
+# Save the filtered point cloud
+o3d.io.write_point_cloud(output_ply, filtered_pcd)

samples/plot_teach_submaps.py

@@ -17,9 +17,17 @@
 
     parser = argparse.ArgumentParser(prog = 'Plot Point Clouds Path',
                             description = 'Plots point clouds')
-    parser.add_argument('-g', '--graph', default=os.getenv("VTRDATA"), help="The filepath to the pose graph folder. (Usually /a/path/graph)")      # option that takes a value
+    parser.add_argument('-g', '--graph', default=os.getenv("VTRDATA"), help="The filepath to the pose graph folder. (Usually /a/path/graph)")
+    parser.add_argument('--save_pc', type=lambda x: (str(x).lower() == 'true'), default=False, help="Whether to save point clouds (True/False).")
+    parser.add_argument('--save_dir', type=str, default=None, help="Directory to save point clouds (required if --save_pc is True).")
     args = parser.parse_args()
 
+    if args.save_pc and not args.save_dir:
+        parser.error("--save_dir must be specified if --save_pc is True.")
+
+    if args.save_pc and not os.path.exists(args.save_dir):
+        os.makedirs(args.save_dir)
+
     factory = Rosbag2GraphFactory(args.graph)
 
     test_graph = factory.buildGraph()
@@ -33,7 +41,6 @@
     live_2_map = []
     map_2_live = []
 
-
     first = True
     paused = False
     def toggle(vis):
@@ -49,13 +56,18 @@ def toggle(vis):
     vis.poll_events()
     vis.update_renderer()
 
+    cloud_count = 0
+
     for i in range(test_graph.major_id + 1):
         v_start = test_graph.get_vertex((i, 0))
         paused = True
         vertices = list(TemporalIterator(v_start))
         vertices_to_plot = vertices[:-10] if len(vertices) > 10 else vertices
 
-        for vertex, e in vertices_to_plot:
+        # Accumulate all points for this major_id
+        accumulated_points = []
+
+        for idx, (vertex, e) in enumerate(vertices_to_plot):
 
             new_points, map_ptr = extract_map_from_vertex(test_graph, vertex)
 
@@ -76,6 +88,18 @@ def toggle(vis):
             else:
                 pcd.paint_uniform_color((0.1*vertex.run, 0.25*vertex.run, 0.45))
 
+            # Save every 20th point cloud if requested
+            if args.save_pc and (cloud_count % 20 == 0):
+                filename = os.path.join(args.save_dir, f"cloud_{cloud_count:05d}.ply")
+                o3d.io.write_point_cloud(filename, pcd)
+                print(f"Saved point cloud to {filename}")
+
+            # Accumulate all points for this major_id
+            if args.save_pc:
+                accumulated_points.append(new_points.T)
+
+            cloud_count += 1
+
             # Create coordinate frame for the vertex
             frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=robot_position)
 
@@ -92,5 +116,16 @@ def toggle(vis):
                 vis.poll_events()
                 vis.update_renderer()
 
+        # Save accumulated point cloud for this major_id
+        if args.save_pc and accumulated_points:
+            all_points = np.vstack(accumulated_points)
+            acc_pcd = o3d.geometry.PointCloud()
+            acc_pcd.points = o3d.utility.Vector3dVector(all_points)
+            acc_filename = os.path.join(args.save_dir, f"accumulated_major_{i:03d}.ply")
+            o3d.io.write_point_cloud(acc_filename, acc_pcd)
+            print(f"Saved accumulated point cloud for major_id {i} to {acc_filename}")
+
+    print("Finished processing all point clouds.")
+
     vis.run()
     vis.destroy_window()

src/vtr_odom_extractor/relative_export_odometry_path.py

@@ -9,6 +9,28 @@
 from scipy.spatial.transform import Rotation as R
 import matplotlib.pyplot as plt
 
+def plot_odometry_path(positions):
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+    ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], label='Odometry Path')
+    ax.set_xlabel('X')
+    ax.set_ylabel('Y')
+    ax.set_zlabel('Z')
+
+    max_range = np.array([positions[:,0].max()-positions[:,0].min(), 
+                          positions[:,1].max()-positions[:,1].min(), 
+                          positions[:,2].max()-positions[:,2].min()]).max()
+    mid_x = (positions[:,0].max()+positions[:,0].min()) * 0.5
+    mid_y = (positions[:,1].max()+positions[:,1].min()) * 0.5
+    mid_z = (positions[:,2].max()+positions[:,2].min()) * 0.5
+    ax.set_xlim(mid_x - max_range/2, mid_x + max_range/2)
+    ax.set_ylim(mid_y - max_range/2, mid_y + max_range/2)
+    ax.set_zlim(mid_z - max_range/2, mid_z + max_range/2)
+
+    ax.legend()
+    plt.title('Odometry Path')
+    plt.show()
+
 def export_relative_transforms(graph_path, output_path):
     if os.path.isdir(output_path):
         output_path_mat = os.path.join(output_path, "relative_transforms_mat_test.csv")
@@ -122,15 +144,7 @@ def export_relative_transforms(graph_path, output_path):
     actual_positions = np.array(actual_positions)
 
     # Plot the path
-    fig = plt.figure()
-    ax = fig.add_subplot(111, projection='3d')
-    ax.plot(actual_positions[:, 0], actual_positions[:, 1], actual_positions[:, 2], label='Odometry Path')
-    ax.set_xlabel('X')
-    ax.set_ylabel('Y')
-    ax.set_zlabel('Z')
-    ax.legend()
-    plt.title('Odometry Path')
-    plt.show()
+    plot_odometry_path(actual_positions)
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description="Export odometry path from pose graph.")