Merge branch 's2025_EstCal' of https://github.com/krishauser/GEMstack into s2025_EstCal

michalj1 · michalj1 · commit 03f084bcaef9 · 2025-04-07T14:04:31.000-05:00
diff --git a/GEMstack/knowledge/calibration/gem_e4_oak_in.yaml b/GEMstack/knowledge/calibration/gem_e4_oak_in.yaml
@@ -1,2 +1,2 @@
 center: [573.37109375, 363.700927734375]
-focal: [684.8333129882812, 684.6096801757812, 1.0]
+focal: [684.8333129882812, 684.6096801757812]
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/.gitignore b/GEMstack/offboard/calibration/calibration_by_segmentation/.gitignore
@@ -0,0 +1,3 @@
+sam_vit_h.pth
+
+!.gitignore
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/CalibAnything b/GEMstack/offboard/calibration/calibration_by_segmentation/CalibAnything
@@ -0,0 +1 @@
+Subproject commit 4122ea18abe1831b99abbadce91362571a9b5158
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/calib.bash b/GEMstack/offboard/calibration/calibration_by_segmentation/calib.bash
@@ -0,0 +1,12 @@
+data=$1
+
+wget -c -O sam_vit_h.pth https://dl.fbaipublicfiles.com/segment_anything/sam_vit_h_4b8939.pth 
+
+echo 'running segmentation...'
+python3 segment-anything scripts/amg.py --checkpoint sam_vit_h.pth --model-type vit_h --input $data/images --output $data/masks/ --stability-score-thresh 0.9 --box-nms-thresh 0.5 --stability-score-offset 0.9
+
+echo 'reprocess segmentation...'
+python3 CalibAnything/processed_mask.py -i $data/masks -o $data/processed_masks/
+
+echo 'running calibration...'
+CalibAnything/bin/run_lidar2camera $data/calib.json
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/calib.json b/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/calib.json
@@ -0,0 +1,58 @@
+{
+  "cam_K": {
+    "rows": 3,
+    "cols": 3,
+    "data": [
+      [1180.753804, 0.000000, 934.859447],
+      [0.000000, 1177.034929, 607.266974],
+      [0.000000, 0.000000   , 1.000000  ]
+    ]
+  },
+  "cam_dist": {
+    "cols": 5,
+    "data": [-0.244851, 0.082024, 0.000429, -0.001245, 0.000000]
+  },
+  "T_lidar_to_cam":{
+    "rows": 4,
+    "cols": 4,
+    "data": [
+     [ 0.72 , -0.694,  0.014,  0.12],
+     [-0.166, -0.191, -0.967,  0.09],
+     [ 0.673,  0.694, -0.253, -1.17],
+     [ 0.0  ,  0.0  ,  0.0  ,  1.0 ]
+    ]
+  },
+  "img_folder": "images",
+  "mask_folder": "processed_masks",
+  "pc_folder": "pc",
+  "img_format": ".png",
+  "pc_format": ".pcd",
+  "file_name": [
+    "000000",
+    "000001",
+    "000002",
+    "000003",
+    "000004"
+  ],
+  "params": {
+    "min_plane_point_num": 2000,
+    "cluster_tolerance": 0.25,
+    "search_num": 4000,
+    "search_range": {
+      "rot_deg": 5,
+      "trans_m": 0.5
+    },
+    "point_range":{
+      "top": 0.4,
+      "bottom": 1.0
+    },
+    "down_sample":{
+      "is_valid": true,
+      "voxel_m": 0.05
+    },
+    "thread": {
+      "is_multi_thread": true,
+      "num_thread": 4
+    }
+  }
+}
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/images/.gitignore b/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/images/.gitignore
@@ -0,0 +1,3 @@
+*
+
+!.gitignore
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/masks/.gitignore b/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/masks/.gitignore
@@ -0,0 +1,3 @@
+*
+
+!.gitignore
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/pc/.gitignore b/GEMstack/offboard/calibration/calibration_by_segmentation/data/fl/pc/.gitignore
@@ -0,0 +1,3 @@
+*
+
+!.gitignore
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/npz_to_pcd.py b/GEMstack/offboard/calibration/calibration_by_segmentation/npz_to_pcd.py
@@ -0,0 +1,58 @@
+#%%
+#%%
+import numpy as np
+import open3d as o3d
+import os
+import argparse
+
+def npz_to_pcd(input_dir, output_dir, data_key='arr_0'):
+    # Create output directory if it doesn't exist
+    os.makedirs(output_dir, exist_ok=True)
+
+    # Iterate over all .npz files in the input directory
+    for npz_file in os.listdir(input_dir):
+        if npz_file.endswith(".npz"):
+            npz_path = os.path.join(input_dir, npz_file)
+            output_pcd_path = os.path.join(output_dir, npz_file.replace('.npz', '.pcd'))
+
+            try:
+                # Load data from .npz file
+                data = np.load(npz_path)
+                if data_key not in data:
+                    print(f"Skipping {npz_file}: Key '{data_key}' not found.")
+                    continue
+
+                point_cloud_np = data[data_key]
+
+                # Validate shape (Nx3 for XYZ, Nx6 for XYZRGB, etc.)
+                if point_cloud_np.ndim != 2 or point_cloud_np.shape[1] < 3:
+                    print(f"Skipping {npz_file}: Invalid shape {point_cloud_np.shape}.")
+                    continue
+
+                # Create Open3D point cloud object
+                pcd = o3d.geometry.PointCloud()
+
+                # Assign points (required: Nx3 array)
+                pcd.points = o3d.utility.Vector3dVector(point_cloud_np[:, :3])
+
+                # Optionally assign colors (if available, e.g., Nx6 array with RGB)
+                if point_cloud_np.shape[1] >= 6:
+                    # RGB values (assuming they are stored as 0-255 integers)
+                    colors = point_cloud_np[:, 3:6] / 255.0  # Normalize to [0, 1] for Open3D
+                    pcd.colors = o3d.utility.Vector3dVector(colors)
+
+                # Save to .pcd (supports ASCII or binary)
+                o3d.io.write_point_cloud(output_pcd_path, pcd, write_ascii=True)
+                print(f"Converted: {npz_file} -> {output_pcd_path}")
+
+            except Exception as e:
+                print(f"Failed to convert {npz_file}: {str(e)}")
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Convert .npz to .pcd using Open3D")
+    parser.add_argument("--input_dir", type=str, required=True, help="Input directory with .npz files")
+    parser.add_argument("--output_dir", type=str, required=True, help="Output directory for .pcd files")
+    parser.add_argument("--data_key", type=str, default='points', help="Key for point cloud data in .npz files")
+    args = parser.parse_args()
+
+    npz_to_pcd(args.input_dir, args.output_dir, args.data_key)
diff --git a/GEMstack/offboard/calibration/calibration_by_segmentation/segment-anything b/GEMstack/offboard/calibration/calibration_by_segmentation/segment-anything
@@ -0,0 +1 @@
+Subproject commit dca509fe793f601edb92606367a655c15ac00fdf
diff --git a/GEMstack/offboard/calibration/play_with_pycolmap.py b/GEMstack/offboard/calibration/play_with_pycolmap.py
@@ -44,7 +44,6 @@ def pc_relative(pc0,pc1):
     sift_options=extract_options,
     # device=pycolmap._core.Device.cuda
 )
-
 # 2. Feature matching. match_sequential tries matching only on pairs close in filename ordering 
 match_options = pycolmap.SequentialMatchingOptions()
 match_options.overlap = 3   # how many subsequent images should be tried matching. 
@@ -138,7 +137,6 @@ def load_scene(path):
     sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s
     return sc
 
-#%%
 #%%
 # get lidar pointcloud
 sc = load_scene(root + f"/data/calib1/pc/ouster1.npz")
diff --git a/GEMstack/offboard/calibration/run_img2pc.bash b/GEMstack/offboard/calibration/run_img2pc.bash
@@ -1,8 +1,8 @@
 root='/mnt/GEMstack'
 
 python3 $root/GEMstack/offboard/calibration/img2pc.py \
-    --img_path $root/data/color32.png \
-    --pc_path $root/data/lidar32.npz \
+    --img_path $root/data/calib1/img/fl/fl11.png \
+    --pc_path $root/data/calib1/pc/ouster11.npz \
     --pc_transform_path $root/GEMstack/knowledge/calibration/gem_e4_ouster.yaml \
     --img_intrinsics_path $root/GEMstack/knowledge/calibration/gem_e4_oak_in.yaml \
     --n_features 4 \
diff --git a/GEMstack/offboard/calibration/test_transforms.py b/GEMstack/offboard/calibration/test_transforms.py
@@ -0,0 +1,207 @@
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation as R
+from matplotlib.widgets import Slider
+
+keep_running = True
+
+# Load LiDAR points
+lidar_data = np.load("./data/ouster32.npz")  # Update filename if needed
+lidar_points = lidar_data['arr_0']  # Ensure the correct key
+
+# Load Camera Image
+image = cv2.imread("./data/fr_rect32.png")  # Update filename if needed
+image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)  # Convert BGR to RGB
+
+# Transformation Matrix
+T_lidar_camera = np.array(
+    [[-0.65266466, -0.75745371, -0.01710912, -0.54157703],
+     [-0.08467423,  0.09536332, -0.99183472, -0.37038288],
+     [ 0.75290047, -0.64588677, -0.12637708,  0.0245309 ],
+     [ 0.        ,  0.        ,  0.        ,  1.        ]]
+)
+
+# Convert LiDAR points to homogeneous coordinates
+num_points = lidar_points.shape[0]
+lidar_homogeneous = np.hstack((lidar_points, np.ones((num_points, 1))))  # (N, 4)
+
+# Transform LiDAR points to Camera Frame
+lidar_points_camera = (T_lidar_camera @ lidar_homogeneous.T).T  # (N, 4)
+
+
+# intrinsic matrix
+K = np.array([
+    [1180.753804,  0.        , 934.859447],
+    [ 0.        , 1177.034929, 607.266974],
+    [ 0.        ,  0.        ,  1.       ]
+], dtype=np.float32)
+
+# Extract 3D points in camera frame
+X_c = lidar_points_camera[:, 0]
+Y_c = lidar_points_camera[:, 1]
+Z_c = lidar_points_camera[:, 2]  # Depth
+
+# Avoid division by zero
+valid = Z_c > 0
+X_c, Y_c, Z_c = X_c[valid], Y_c[valid], Z_c[valid]
+
+# Project points onto image plane
+u = (K[0, 0] * X_c / Z_c) + K[0, 2]
+v = (K[1, 1] * Y_c / Z_c) + K[1, 2]
+
+# Filter points within image bounds
+img_h, img_w, _ = image.shape
+valid_pts = (u >= 0) & (u < img_w) & (v >= 0) & (v < img_h)
+u, v = u[valid_pts], v[valid_pts]
+
+# Create space for sliders
+plt.ion()
+fig, ax = plt.subplots()
+fig.subplots_adjust(bottom=0.4)
+scale_ax = fig.add_axes([0.25, 0.35, 0.65, 0.03])
+x_rot_ax = fig.add_axes([0.25, 0.3, 0.65, 0.03])
+y_rot_ax = fig.add_axes([0.25, 0.25, 0.65, 0.03])
+z_rot_ax = fig.add_axes([0.25, 0.2, 0.65, 0.03])
+x_trans_ax = fig.add_axes([0.25, 0.15, 0.65, 0.03])
+y_trans_ax = fig.add_axes([0.25, 0.1, 0.65, 0.03])
+z_trans_ax = fig.add_axes([0.25, 0.05, 0.65, 0.03])
+
+# Plot projected points on camera image
+ax.imshow(image)
+dots = ax.scatter(u, v, s=2, c='cyan', alpha=0.2)  # Dots for projected points
+ax.set_title("Projected LiDAR Points on Camera Image", pad=0)
+
+# Make a slider to control the scale
+scale = Slider(
+    ax=scale_ax,
+    label="Scale",
+    valmin=0.75,
+    valmax=1.25,
+    valinit=1,
+    orientation="horizontal"
+)
+
+# Make sliders to control the rotation
+x_rot = Slider(
+    ax=x_rot_ax,
+    label="X Rotation",
+    valmin=-30,
+    valmax=30,
+    valinit=0,
+    orientation="horizontal"
+)
+
+y_rot = Slider(
+    ax=y_rot_ax,
+    label="Y Rotation",
+    valmin=-30,
+    valmax=30,
+    valinit=0,
+    orientation="horizontal"
+)
+
+z_rot = Slider(
+    ax=z_rot_ax,
+    label="Z Rotation",
+    valmin=-30,
+    valmax=30,
+    valinit=0,
+    orientation="horizontal"
+)
+
+# Make sliders to control the scale
+x_trans = Slider(
+    ax=x_trans_ax,
+    label="X Translation",
+    valmin=-10,
+    valmax=10,
+    valinit=0,
+    orientation="horizontal"
+)
+
+y_trans = Slider(
+    ax=y_trans_ax,
+    label="Y Translation",
+    valmin=-10,
+    valmax=10,
+    valinit=0,
+    orientation="horizontal"
+)
+
+z_trans = Slider(
+    ax=z_trans_ax,
+    label="Z Translation",
+    valmin=-10,
+    valmax=10,
+    valinit=0,
+    orientation="horizontal"
+)
+
+def update(val):
+    global T_lidar_camera
+    modified = np.copy(T_lidar_camera)
+    rotation_mat = R.from_euler('xyz', [x_rot.val, y_rot.val, z_rot.val], degrees=True).as_matrix()
+    scale_mat = np.array([[scale.val, 0, 0], [0, scale.val, 0], [0, 0, scale.val]])
+    translation_vec = np.array([x_trans.val, y_trans.val, z_trans.val])
+    modified[:3, :3] = T_lidar_camera[:3, :3] @ rotation_mat @ scale_mat
+    modified[:3, 3] = T_lidar_camera[:3, 3] + translation_vec
+
+    # Transform LiDAR points to Camera Frame
+    lidar_points_camera = (modified @ lidar_homogeneous.T).T  # (N, 4)
+
+
+    # intrinsic matrix
+    K = np.array([
+        [1180.753804,  0.        , 934.859447],
+        [ 0.        , 1177.034929, 607.266974],
+        [ 0.        ,  0.        ,  1.       ]
+    ], dtype=np.float32)
+
+    # Extract 3D points in camera frame
+    X_c = lidar_points_camera[:, 0]
+    Y_c = lidar_points_camera[:, 1]
+    Z_c = lidar_points_camera[:, 2]  # Depth
+
+    # Avoid division by zero
+    valid = Z_c > 0
+    X_c, Y_c, Z_c = X_c[valid], Y_c[valid], Z_c[valid]
+
+    # Project points onto image plane
+    u = (K[0, 0] * X_c / Z_c) + K[0, 2]
+    v = (K[1, 1] * Y_c / Z_c) + K[1, 2]
+
+    # Filter points within image bounds
+    img_h, img_w, _ = image.shape
+    valid_pts = (u >= 0) & (u < img_w) & (v >= 0) & (v < img_h)
+    u, v = u[valid_pts], v[valid_pts]
+    dots.set_offsets(np.c_[u, v])
+    plt.draw()
+
+
+# register the update function with each slider
+scale.on_changed(update)
+x_rot.on_changed(update)
+y_rot.on_changed(update)
+z_rot.on_changed(update)
+x_trans.on_changed(update)
+y_trans.on_changed(update)
+z_trans.on_changed(update)
+
+plt.draw()
+while keep_running:
+    try:
+        if plt.get_fignums():
+            plt.pause(0.1)
+        else:
+            keep_running = False
+    except:
+        keep_running = False
+
+modified = np.copy(T_lidar_camera)
+rotation_mat = R.from_euler('xyz', [x_rot.val, y_rot.val, z_rot.val], degrees=True).as_matrix()
+scale_mat = np.array([[scale.val, 0, 0], [0, scale.val, 0], [0, 0, scale.val]])
+translation_vec = np.array([x_trans.val, y_trans.val, z_trans.val])
+modified[:3, :3] = T_lidar_camera[:3, :3] @ rotation_mat @ scale_mat
+modified[:3, 3] = T_lidar_camera[:3, 3] + translation_vec
+print(modified)
diff --git a/GEMstack/offboard/calibration/tools/save_cali.py b/GEMstack/offboard/calibration/tools/save_cali.py
diff --git a/GEMstack/offboard/calibration/undistort_images.py b/GEMstack/offboard/calibration/undistort_images.py

Original file line number	Diff line number	Diff line change
`@@ -1,2 +1,2 @@`
`1`	`1`	`center: [573.37109375, 363.700927734375]`
`2`		`-focal: [684.8333129882812, 684.6096801757812, 1.0]`
	`2`	`+focal: [684.8333129882812, 684.6096801757812]`
Original file line number	Diff line number	Diff line change
`@@ -0,0 +1 @@`
	`1`	`+Subproject commit 4122ea18abe1831b99abbadce91362571a9b5158`
Original file line number	Diff line number	Diff line change
`@@ -0,0 +1 @@`
	`1`	`+Subproject commit dca509fe793f601edb92606367a655c15ac00fdf`