From 51a81508a1d258333ef633853ab1e2bebcc93ed3 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Wed, 19 Feb 2025 23:54:50 +0000 Subject: [PATCH 01/22] correct tx,ty --- .../calibration/make_gem_e4_ouster_v2.py | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py index 6e0b25940..bad0f1090 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -1,10 +1,10 @@ #%% -import os import sys import math import numpy as np from scipy.spatial.transform import Rotation as R -os.getcwd() +import pyvista as pv +import matplotlib.pyplot as plt VIS = False # True to show visuals #%% things to extract @@ -13,17 +13,16 @@ #%%============================================================== #======================= util functions ========================= #================================================================ -if VIS: - import pyvista as pv - import matplotlib.pyplot as plt def vis(title='', ratio=1): print(title) pv.set_jupyter_backend('client') plotter = pv.Plotter(notebook=True) - plotter.camera.position = (-20*ratio,0,20*ratio) - plotter.camera.focal_point = (0,0,0) plotter.show_axes() class foo: + def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): + plotter.camera.position = pos + plotter.camera.focal_point = foc + return self def add_pc(self,pc,ratio=ratio,**kargs): plotter.add_mesh( pv.PolyData(pc*ratio), @@ -46,8 +45,11 @@ def add_box(self,bound,trans,ratio=ratio): def show(self): plotter.show() return self + def close(self): + plotter.close() + return None - return foo() + return foo().set_cam() def load_scene(path): sc = np.load(path)['arr_0'] sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s @@ -123,7 +125,7 @@ def pc_diff(pc0,pc1,tol=0.1): #================================================================ #%% load scene for ground plane -sc = load_scene('/mount/wp/GEMstack/data/lidar70.npz') +sc = load_scene('/mount/wp/GEMstack/data/lidar1.npz') # %% we crop to keep the ground cropped_sc = crop(sc,iz = (-3,-2)) @@ -204,9 +206,9 @@ def pc_diff(pc0,pc1,tol=0.1): # tx = ty = 0 # hx,hy = np.median(diff,axis=0)[:2] # rz = -np.arctan2(hy,hx) -tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ -ty = - inter rz = - math.atan(c) +tx = 2.56 - 1.46 # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +ty = - inter * math.cos(rz) if VIS: p1 = (0,inter,0) @@ -224,10 +226,9 @@ def pc_diff(pc0,pc1,tol=0.1): rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() cal_sc1 = sc1 @ rot.T + [tx,ty,tz] - # projection - # cal_sc1[:,1] = 0 v = vis(ratio=100) - v.add_pc(cal_sc1*[0,1,1],color='blue') + proj = [1,1,1] + v.add_pc(cal_sc1*proj,color='blue') v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) v.show() # the yellow box should be 11 inches above the ground From 3891eedef59a3b450d8beaead04b7309d36517be Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Wed, 19 Feb 2025 16:01:55 -0800 Subject: [PATCH 02/22] updating tx in lidar to camera --- GEMstack/offboard/calibration/lidar_to_camera_manual.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py index 4d3cc9719..a416f06fc 100644 --- a/GEMstack/offboard/calibration/lidar_to_camera_manual.py +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -14,7 +14,7 @@ lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 -tx,ty,tz = -1.1, 0.037735827433173136, 1.953202227766785 +tx,ty,tz = 1.1, 0.037735827433173136, 1.953202227766785 rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) From 63432ef46beb27bbbb6cfea57ef07443e9efd835 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Thu, 20 Feb 2025 00:31:15 +0000 Subject: [PATCH 03/22] lidar->vehecle matrix. and update manual pointpicking --- .../knowledge/calibration/gem_e4_ouster.yaml | 6 +++-- .../calibration/lidar_to_camera_manual.py | 24 ++++++++++--------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/GEMstack/knowledge/calibration/gem_e4_ouster.yaml b/GEMstack/knowledge/calibration/gem_e4_ouster.yaml index 5987373a6..4ab6b42e0 100644 --- a/GEMstack/knowledge/calibration/gem_e4_ouster.yaml +++ b/GEMstack/knowledge/calibration/gem_e4_ouster.yaml @@ -1,3 +1,5 @@ reference: rear_axle_center # rear axle center -position: [1.10,0,2.03] # meters, calibrated by Hang's watchful eye -rotation: [[1,0,0],[0,1,0],[0,0,1]] #rotation matrix mapping lidar frame to vehicle frame \ No newline at end of file +position: [1.1, 0.03773044170906172, 1.9525244316515322] +rotation: [[ 0.99941328, 0.02547416, 0.02289458], + [-0.02530855, 0.99965159, -0.00749488], + [-0.02307753, 0.00691106, 0.99970979]] \ No newline at end of file diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py index 4d3cc9719..72496807c 100644 --- a/GEMstack/offboard/calibration/lidar_to_camera_manual.py +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -14,7 +14,7 @@ lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 -tx,ty,tz = -1.1, 0.037735827433173136, 1.953202227766785 +tx,ty,tz = 1.1, 0.03773044170906172, 1.9525244316515322 rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) @@ -25,6 +25,8 @@ [ 0. , 0. , 1. ] ], dtype=np.float32) +N = 8 + #%% # blurred = cv2.GaussianBlur(img, (5, 5), 0) # edges = cv2.Canny(blurred, threshold1=50, threshold2=150) @@ -33,10 +35,10 @@ #%% import pyvista as pv -def vis(title='', ratio=1): +def vis(title='', ratio=1,notebook=False): print(title) pv.set_jupyter_backend('client') - plotter = pv.Plotter(notebook=True) + plotter = pv.Plotter(notebook=notebook) plotter.show_axes() class foo: def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): @@ -86,10 +88,10 @@ def crop(pc,ix=None,iy=None,iz=None): lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) -vis().add_pc(lidar_post).show() +vis(notebook=True).add_pc(lidar_post).show() #%% -def pick_4_img(img): +def pick_n_img(img,n=4): corners = [] # Reset the corners list def click_event(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: @@ -101,7 +103,7 @@ def click_event(event, x, y, flags, param): cv2.setMouseCallback('Image', click_event, img) while True: - if len(corners) == 4: + if len(corners) == n: break if cv2.waitKey(1) & 0xFF == ord('q'): return None @@ -109,15 +111,15 @@ def click_event(event, x, y, flags, param): cv2.destroyAllWindows() return corners -cpoints = np.array(pick_4_img(img)).astype(float) +cpoints = np.array(pick_n_img(img,N)).astype(float) print(cpoints) #%% -def pick_4_pc(point_cloud): +def pick_n_pc(point_cloud,n=4): points = [] def cb(pt,*args): points.append(pt) - while len(points)!=4: + while len(points)!=n: points = [] cloud = pv.PolyData(point_cloud) plotter = pv.Plotter(notebook=False) @@ -128,7 +130,7 @@ def cb(pt,*args): plotter.show() return points -lpoints = np.array(pick_4_pc(lidar_post)) +lpoints = np.array(pick_n_pc(lidar_post,N)) print(lpoints) # %% success, rvec, tvec = cv2.solvePnP(lpoints, cpoints, camera_in, None) @@ -182,7 +184,7 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) camera_points = depth_to_points(depth_img, camera_in) -v=vis() +v=vis(notebook=False) v.add_pc(np.pad(lidar_points,((0,0),(0,1)),constant_values=1)@lidar_ex.T@T.T[:,:3],color='blue') v.add_pc(camera_points,color='red') v.show() From 1b4435171e1bd88116cd81dd4d7197d23ea32833 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Thu, 20 Feb 2025 01:06:02 +0000 Subject: [PATCH 04/22] camera to vehicle by point selecting. --- ..._manual.py => camera_to_vehicle_manual.py} | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) rename GEMstack/offboard/calibration/{lidar_to_camera_manual.py => camera_to_vehicle_manual.py} (87%) diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py similarity index 87% rename from GEMstack/offboard/calibration/lidar_to_camera_manual.py rename to GEMstack/offboard/calibration/camera_to_vehicle_manual.py index ec12d1131..b578d8bc7 100644 --- a/GEMstack/offboard/calibration/lidar_to_camera_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -4,9 +4,13 @@ import matplotlib.pyplot as plt import numpy as np -rgb_path = '/mount/wp/GEMstack/data/color21.png' -depth_path = '/mount/wp/GEMstack/data/depth21.tif' -lidar_path = '/mount/wp/GEMstack/data/lidar21.npz' +N = 4 #how many point pairs you want to select +rgb_path = '/mount/wp/GEMstack/data/color20.png' +depth_path = '/mount/wp/GEMstack/data/depth20.tif' +lidar_path = '/mount/wp/GEMstack/data/lidar20.npz' +# rgb_path = '/mount/wp/GEMstack/data/color25.png' +# depth_path = '/mount/wp/GEMstack/data/depth25.tif' +# lidar_path = '/mount/wp/GEMstack/data/lidar25.npz' img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) @@ -14,11 +18,7 @@ lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 -<<<<<<< HEAD -tx,ty,tz = 1.1, 0.03773044170906172, 1.9525244316515322 -======= tx,ty,tz = 1.1, 0.037735827433173136, 1.953202227766785 ->>>>>>> 3891eedef59a3b450d8beaead04b7309d36517be rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) @@ -29,7 +29,6 @@ [ 0. , 0. , 1. ] ], dtype=np.float32) -N = 8 #%% # blurred = cv2.GaussianBlur(img, (5, 5), 0) @@ -91,7 +90,7 @@ def crop(pc,ix=None,iy=None,iz=None): lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] -lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) +lidar_post = crop(lidar_post,ix=(0,10),iy=(-5,5)) vis(notebook=True).add_pc(lidar_post).show() #%% @@ -188,10 +187,18 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) camera_points = depth_to_points(depth_img, camera_in) -v=vis(notebook=False) -v.add_pc(np.pad(lidar_points,((0,0),(0,1)),constant_values=1)@lidar_ex.T@T.T[:,:3],color='blue') -v.add_pc(camera_points,color='red') +#%% +v2c = T +print('vehicle->camera:',l2c) +c2v = np.linalg.inv(l2c) +print('camera->vehicle:',c2l) + +v=vis(notebook=True) +v.add_pc(lidar_post,color='blue') +v.add_pc(np.pad(camera_points,((0,0),(0,1)),constant_values=1)@c2v.T[:,:3],color='red') v.show() -#%% -print(np.vstack([(lidar_ex.T@T.T[:,:3]).T,[0,0,0,1]])) \ No newline at end of file +v=vis(notebook=True) +v.add_pc(np.pad(lidar_post,((0,0),(0,1)),constant_values=1)@v2c.T[:,:3],color='blue') +v.add_pc(camera_points,color='red') +v.show() \ No newline at end of file From 9c91ffc735c45df410bf61584a20a32cc9b23a5c Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Thu, 20 Feb 2025 01:28:06 +0000 Subject: [PATCH 05/22] dumb typo --- GEMstack/offboard/calibration/camera_to_vehicle_manual.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index b578d8bc7..86758e844 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -189,9 +189,9 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): #%% v2c = T -print('vehicle->camera:',l2c) +print('vehicle->camera:',v2c) c2v = np.linalg.inv(l2c) -print('camera->vehicle:',c2l) +print('camera->vehicle:',c2v) v=vis(notebook=True) v.add_pc(lidar_post,color='blue') From 11907a37e389bf17f9972ddd0ffa5791796c3cee Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Thu, 20 Feb 2025 01:28:53 +0000 Subject: [PATCH 06/22] dumb typo --- GEMstack/offboard/calibration/camera_to_vehicle_manual.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index 86758e844..710124999 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -190,7 +190,7 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): #%% v2c = T print('vehicle->camera:',v2c) -c2v = np.linalg.inv(l2c) +c2v = np.linalg.inv(v2c) print('camera->vehicle:',c2v) v=vis(notebook=True) From fa73970a5fff97bc609c261ffb3e90b53b2f9391 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Fri, 21 Feb 2025 11:13:16 -0800 Subject: [PATCH 07/22] adding testing script and lidar to cam script --- .../calibration/camera_to_vehicle_manual.py | 9 +-- .../offboard/calibration/lidar_to_camera.py | 20 +++++++ .../offboard/calibration/test_transforms.py | 57 +++++++++++++++++++ 3 files changed, 80 insertions(+), 6 deletions(-) create mode 100644 GEMstack/offboard/calibration/lidar_to_camera.py create mode 100644 GEMstack/offboard/calibration/test_transforms.py diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index 710124999..07c204aa1 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -5,12 +5,9 @@ import numpy as np N = 4 #how many point pairs you want to select -rgb_path = '/mount/wp/GEMstack/data/color20.png' -depth_path = '/mount/wp/GEMstack/data/depth20.tif' -lidar_path = '/mount/wp/GEMstack/data/lidar20.npz' -# rgb_path = '/mount/wp/GEMstack/data/color25.png' -# depth_path = '/mount/wp/GEMstack/data/depth25.tif' -# lidar_path = '/mount/wp/GEMstack/data/lidar25.npz' +rgb_path = './data/color20.png' +depth_path = './data/depth20.tif' +lidar_path = './data/lidar20.npz' img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) diff --git a/GEMstack/offboard/calibration/lidar_to_camera.py b/GEMstack/offboard/calibration/lidar_to_camera.py new file mode 100644 index 000000000..d45b7ab3d --- /dev/null +++ b/GEMstack/offboard/calibration/lidar_to_camera.py @@ -0,0 +1,20 @@ +import numpy as np + +T_lidar_vehicle = np.array([[ 0.99939639, 0.02547917, 0.023615, 1.1 ], + [-0.02530848, 0.99965156, -0.00749882, 0.03773583], + [-0.02379784, 0.00689664, 0.999693, 1.95320223], + [ 0., 0., 0., 1. ]]) + +T_camera_vehicle = np.array([[-0.025131, -0.0304479, 0.99922038, 1.78251567], + [-0.99892897, 0.0396095, -0.0239167, 0.18649424], + [-0.0388504, -0.99875123, -0.03141071, 1.5399846 ], + [ 0., 0., 0., 1. ]]) + +# Invert Camera->Vehicle transformation +T_vehicle_camera = np.linalg.inv(T_camera_vehicle) + +T_lidar_camera = T_vehicle_camera @ T_lidar_vehicle + +# Print result +print("Lidar to Camera Transformation Matrix:") +print(T_lidar_camera) diff --git a/GEMstack/offboard/calibration/test_transforms.py b/GEMstack/offboard/calibration/test_transforms.py new file mode 100644 index 000000000..69bdefbc9 --- /dev/null +++ b/GEMstack/offboard/calibration/test_transforms.py @@ -0,0 +1,57 @@ +import numpy as np +import cv2 +import matplotlib.pyplot as plt + +# Load LiDAR points +lidar_data = np.load("./data/lidar1.npz") # Update filename if needed +lidar_points = lidar_data['arr_0'] # Ensure the correct key + +# Load Camera Image +image = cv2.imread("./data/color1.png") # Update filename if needed +image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) # Convert BGR to RGB + +# Transformation Matrix +T_lidar_camera = np.array([ + [0.001090, -0.999489, -0.031941, 0.149698], + [-0.007664, 0.031932, -0.999461, -0.397813], + [0.999970, 0.001334, -0.007625, -0.691405], + [0.000000, 0.000000, 0.000000, 1.000000] +]) + +# 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([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 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] + +# Plot projected points on camera image +plt.imshow(image) +plt.scatter(u, v, s=2, c='r') # Red dots for projected points +plt.title("Projected LiDAR Points on Camera Image") +plt.show() From 5e04c43df0303e759359a0495fe0ce8ff9539311 Mon Sep 17 00:00:00 2001 From: michalj1 Date: Mon, 24 Feb 2025 10:05:46 -0600 Subject: [PATCH 08/22] Add camera transform to yaml file --- GEMstack/knowledge/calibration/gem_e4_oak.yaml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/GEMstack/knowledge/calibration/gem_e4_oak.yaml b/GEMstack/knowledge/calibration/gem_e4_oak.yaml index cb9a6c0d0..ef9bf40a3 100644 --- a/GEMstack/knowledge/calibration/gem_e4_oak.yaml +++ b/GEMstack/knowledge/calibration/gem_e4_oak.yaml @@ -1,3 +1,5 @@ reference: rear_axle_center # rear axle center -rotation: [[0,0,1],[-1,0,0],[0,-1,0]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated -center_position: [1.78,0,1.58] # meters, center camera, guesstimated +rotation: [[-0.025131, -0.0304479, 0.99922038], + [-0.99892897, 0.0396095, -0.0239167], + [-0.0388504, -0.99875123, -0.03141071]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated +center_position: [1.78251567,0.18649424,1.5399846] # meters, center camera, guesstimated From adc4dd5be0edc8bade48b6a9d678b5de6022f092 Mon Sep 17 00:00:00 2001 From: whizkid42 Date: Mon, 24 Feb 2025 17:55:25 -0600 Subject: [PATCH 09/22] Added README.md --- GEMstack/offboard/calibration/README.md | 80 +++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 GEMstack/offboard/calibration/README.md diff --git a/GEMstack/offboard/calibration/README.md b/GEMstack/offboard/calibration/README.md new file mode 100644 index 000000000..0d7d04be0 --- /dev/null +++ b/GEMstack/offboard/calibration/README.md @@ -0,0 +1,80 @@ +# GEMstack Offboard Calibration Suite + +This repository contains tools for offline calibration of LiDAR and camera sensors to the vehicle coordinate system on the GEM E4 platform. The calibration pipeline consists of three stages: + +1. **LiDAR-to-Vehicle** +2. **Camera-to-Vehicle** +3. **LiDAR-to-Camera** + +--- + + +## Calibration Pipeline + +### 1. LiDAR-to-Vehicle Calibration (`make_gem_e4_ouster_v2.py`) +**Method**: +- **Ground Plane Detection**: + 1. Crop LiDAR points near ground (Z ∈ [-3, -2]) + 2. Use RANSAC to fit a plane to ground points + 3. Compute vehicle height (`tz`) using plane equation and axel height offset (0.2794m) + 4. Derive pitch (`rx`) and roll (`ry`) from plane normal vector + +- **Frontal Object Alignment**: + 1. Crop LiDAR data to frontal area (X ∈ [0,20], Y ∈ [-1,1]) + 2. Fit line to object points for yaw (`rz`) estimation + 3. Compute translation (`tx`, `ty`) using vehicle dimensions + +**Usage**: +# Edit LiDAR data paths in script +python3 make_gem_e4_ouster_v2.py + + +### 2. CAMERA-to-Vehicle Calibration (`camera_to_vehicle_manual.py`) +**Method**: + 1. Capture camera intrinsics using camera_info.py (ROS node) + 2. Manually select 4 matching points in RGB image and LiDAR cloud + 3. Solve PnP problem to compute camera extrinsics + +**Usage**: + 1. Get camera intrinsics: + rosrun offboard\calibration\camera_info.py # Prints intrinsic matrix + 2. Update camera_in in script with intrinsics + 3. Run calibration: + python3 camera_to_vehicle_manual.py + + +### 3. LIDAR-to-CAMERA Calibration (`lidar_to_camera.py`) +**Method**: + 1. Invert Camera-to-Vehicle matrix + 2. Multiply with LiDAR-to-Vehicle matrix + +**Usage**: +# Ensure T_lidar_vehicle and T_camera_vehicle matrices are updated +python3 lidar_to_camera.py + + +### Visualization Tools + +**3D Alignment Check**: + 1. Use vis() function in scripts to view calibrated LiDAR/camera clouds + 2. Toggle VIS = True in make_gem_e4_ouster_v2.py for ground plane/object visualization + +**Projection Validation**: + 1. RGB image overlaid with transformed LiDAR points (Z-buffered) + 2. Frontal view comparison of camera and LiDAR data + + +### Assumption and Notes + +1. The sensor data should be time-aligned. +2. The flat surface should be detectable in the Lidar scan +3. Magic Numbers: + Axel height (0.2794m) from physical measurements + Frontal crop areas based on vehicle dimensions +4. Validation: Use pyvista visualizations to verify alignment + + + + + + From 9d4710568cd2863845cd901f6cafe5c605c07b49 Mon Sep 17 00:00:00 2001 From: whizkid42 Date: Mon, 24 Feb 2025 18:02:29 -0600 Subject: [PATCH 10/22] Updated README.md --- GEMstack/offboard/calibration/README.md | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/GEMstack/offboard/calibration/README.md b/GEMstack/offboard/calibration/README.md index 0d7d04be0..a5bf03ad8 100644 --- a/GEMstack/offboard/calibration/README.md +++ b/GEMstack/offboard/calibration/README.md @@ -1,4 +1,4 @@ -# GEMstack Offboard Calibration Suite +# GEMstack Offboard Calibration This repository contains tools for offline calibration of LiDAR and camera sensors to the vehicle coordinate system on the GEM E4 platform. The calibration pipeline consists of three stages: @@ -25,8 +25,8 @@ This repository contains tools for offline calibration of LiDAR and camera senso 3. Compute translation (`tx`, `ty`) using vehicle dimensions **Usage**: -# Edit LiDAR data paths in script -python3 make_gem_e4_ouster_v2.py + +python3 make_gem_e4_ouster_v2.py # Edit LiDAR data paths in script ### 2. CAMERA-to-Vehicle Calibration (`camera_to_vehicle_manual.py`) @@ -49,8 +49,8 @@ python3 make_gem_e4_ouster_v2.py 2. Multiply with LiDAR-to-Vehicle matrix **Usage**: -# Ensure T_lidar_vehicle and T_camera_vehicle matrices are updated -python3 lidar_to_camera.py + +python3 lidar_to_camera.py # Ensure T_lidar_vehicle and T_camera_vehicle matrices are updated ### Visualization Tools @@ -64,6 +64,9 @@ python3 lidar_to_camera.py 2. Frontal view comparison of camera and LiDAR data + + + ### Assumption and Notes 1. The sensor data should be time-aligned. From 66bc2bd1c4c39052ab5269e2648da8724b11847f Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Tue, 25 Feb 2025 11:09:16 +0000 Subject: [PATCH 11/22] improved camera->vehicle --- GEMstack/knowledge/calibration/gem_e4_oak.yaml | 8 ++++---- GEMstack/offboard/calibration/camera_to_vehicle_manual.py | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/GEMstack/knowledge/calibration/gem_e4_oak.yaml b/GEMstack/knowledge/calibration/gem_e4_oak.yaml index ef9bf40a3..b1b27186d 100644 --- a/GEMstack/knowledge/calibration/gem_e4_oak.yaml +++ b/GEMstack/knowledge/calibration/gem_e4_oak.yaml @@ -1,5 +1,5 @@ reference: rear_axle_center # rear axle center -rotation: [[-0.025131, -0.0304479, 0.99922038], - [-0.99892897, 0.0396095, -0.0239167], - [-0.0388504, -0.99875123, -0.03141071]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated -center_position: [1.78251567,0.18649424,1.5399846] # meters, center camera, guesstimated +rotation: [[ 0.00349517, -0.03239524, 0.99946903], +[-0.99996547, 0.00742285, 0.0037375], +[-0.00753999, -0.99944757, -0.03236817]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated +center_position: [ 1.75864913, 0.01238124, 1.54408419] # meters, center camera, guesstimated diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index 07c204aa1..3c61b93fd 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -4,7 +4,7 @@ import matplotlib.pyplot as plt import numpy as np -N = 4 #how many point pairs you want to select +N = 8 #how many point pairs you want to select rgb_path = './data/color20.png' depth_path = './data/depth20.tif' lidar_path = './data/lidar20.npz' From f09bde89b8922d6b8fde2ff6e7d9d8b19400b46a Mon Sep 17 00:00:00 2001 From: aadarshhegde Date: Tue, 25 Feb 2025 18:45:19 -0600 Subject: [PATCH 12/22] removing unnecessary file --- GEMstack/offboard/calibration/icp.py | 179 --------------------------- 1 file changed, 179 deletions(-) delete mode 100644 GEMstack/offboard/calibration/icp.py diff --git a/GEMstack/offboard/calibration/icp.py b/GEMstack/offboard/calibration/icp.py deleted file mode 100644 index 376c285a2..000000000 --- a/GEMstack/offboard/calibration/icp.py +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env python3 - -import os -import glob -import numpy as np -import cv2 -import open3d as o3d - -# --- Adjust these to match your intrinsics and data details --- -INTRINSICS = np.array([ - [684.83331299, 0. , 573.37109375], - [ 0. , 684.60968018, 363.70092773], - [ 0. , 0. , 1. ] -], dtype=np.float32) - -# Depth scale: how to go from raw .tif values to actual "Z" in meters. -# In your capture code, you used: dimage = (dimage/4000*0xffff) -# Possibly each pixel is stored with range 0..65535. Adjust as needed. -DEPTH_SCALE = 4000.0 # Example factor if your depth was in mm or a certain scale. - -def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): - """ - Convert a single-channel depth image into an Nx3 array of 3D points - in the camera coordinate system. - - depth_img: 2D array of type uint16 or float with depth values - - intrinsics: 3x3 camera matrix - """ - fx = intrinsics[0,0] - fy = intrinsics[1,1] - cx = intrinsics[0,2] - cy = intrinsics[1,2] - - # Indices of each pixel - h, w = depth_img.shape - i_range = np.arange(h) - j_range = np.arange(w) - jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) - - # Flatten - ii = ii.flatten().astype(np.float32) - jj = jj.flatten().astype(np.float32) - depth = depth_img.flatten().astype(np.float32) - - # Filter out zero / invalid depths - valid_mask = (depth > 0) - ii = ii[valid_mask] - jj = jj[valid_mask] - depth = depth[valid_mask] - - # Reproject to 3D (camera frame) - # X = (x - cx) * Z / fx, Y = (y - cy) * Z / fy, Z = depth - # Note: Z must be in meters or consistent units - z = depth / DEPTH_SCALE # Convert from your .tif scale to real meters - x = (jj - cx) * z / fx - y = (ii - cy) * z / fy - points3d = np.stack((x, y, z), axis=-1) # shape (N,3) - - return points3d - -def load_lidar_points(npz_file: str): - """ - Load the Nx3 LiDAR points from a .npz file created by 'np.savez'. - """ - data = np.load(npz_file) - # The capture code used: np.savez(lidar_fn, pc) - # So 'data' might have the default key 'arr_0' or 'pc' if named - # Inspect data.files to see. Let's assume 'arr_0' or single key: - arr_key = data.files[0] - points = data[arr_key] - # shape check, must be Nx3 or Nx4, ... - return points - -def make_open3d_pcd(points: np.ndarray): - """ - Convert Nx3 numpy array into an Open3D point cloud object. - """ - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(points) - return pcd - -def perform_icp(camera_pcd: o3d.geometry.PointCloud, - lidar_pcd: o3d.geometry.PointCloud): - """ - Perform local ICP alignment of camera_pcd (source) to lidar_pcd (target). - Returns a transformation 4x4 that maps camera -> lidar (or vice versa). - """ - # 1) Estimate normals if you want point-to-plane - lidar_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( - radius=1.0, max_nn=30)) - camera_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( - radius=1.0, max_nn=30)) - - # 2) Initial guess: identity or something better if you have one - init_guess = np.eye(4) - - # 3) Choose a threshold for inlier distance - threshold = 0.5 # adjust based on your environment scale - - # 4) Run ICP (point-to-plane or point-to-point) - print("[ICP] Running ICP alignment ...") - result = o3d.pipelines.registration.registration_icp( - camera_pcd, # source - lidar_pcd, # target - threshold, - init_guess, - estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane() - ) - - print("[ICP] Done. Fitness = %.4f, RMSE = %.4f" % (result.fitness, result.inlier_rmse)) - print("[ICP] Transformation:\n", result.transformation) - return result.transformation - -def main(folder='data'): - color_files = sorted(glob.glob(os.path.join(folder, "color*.png"))) - icp_results = [] # to store (index, transform, fitness, rmse) - - for color_path in color_files: - # Extract index from filename, e.g. color10.png -> 10 - basename = os.path.basename(color_path) - idx_str = basename.replace("color","").replace(".png","") - - if int(idx_str) in range(77): - depth_path = os.path.join(folder, f"depth{idx_str}.tif") - lidar_path = os.path.join(folder, f"lidar{idx_str}.npz") - - if not (os.path.exists(depth_path) and os.path.exists(lidar_path)): - continue - - # Load depth / convert to 3D - depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) - if depth_img is None: - continue - camera_points = depth_to_points(depth_img, INTRINSICS) - camera_pcd = make_open3d_pcd(camera_points) - - # Load LiDAR - lidar_points = load_lidar_points(lidar_path) - lidar_pcd = make_open3d_pcd(lidar_points) - - lidar_pcd.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid( - radius=1.0, # Adjust based on your scene scale - max_nn=30 - ) - ) - - # Also estimate normals on the Camera (source) cloud (recommended) - camera_pcd.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid( - radius=1.0, - max_nn=30 - ) - ) - - # Now you can run Point-to-Plane ICP - init_guess = np.eye(4) - threshold = 0.5 # or some appropriate distance - result_icp = o3d.pipelines.registration.registration_icp( - camera_pcd, - lidar_pcd, - threshold, - init_guess, - o3d.pipelines.registration.TransformationEstimationPointToPlane() - ) - - print("ICP result:", result_icp.transformation) - print("Fitness:", result_icp.fitness, " RMSE:", result_icp.inlier_rmse) - - # After processing all frames, we can analyze or average - if len(icp_results) > 0: - # Example: pick the best frame by highest fitness - best_frame = max(icp_results, key=lambda x: x[2]) # x[2] is fitness - print("\nBest frame by fitness was index=", best_frame[0], - " with fitness=", best_frame[2], " rmse=", best_frame[3]) - else: - print("No frames processed properly.") - -if __name__ == "__main__": - main() \ No newline at end of file From af7b5ec995cde6b52b261742c19e0cc2a6a5fa15 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 <89495844+AadarshHegde123@users.noreply.github.com> Date: Tue, 25 Feb 2025 18:49:24 -0600 Subject: [PATCH 13/22] Rename make_gem_e4_ouster_v2.py to lidar_to_vehicle.py --- .../calibration/{make_gem_e4_ouster_v2.py => lidar_to_vehicle.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename GEMstack/offboard/calibration/{make_gem_e4_ouster_v2.py => lidar_to_vehicle.py} (100%) diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/lidar_to_vehicle.py similarity index 100% rename from GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py rename to GEMstack/offboard/calibration/lidar_to_vehicle.py From c247a4393a93d4178728573ca296df40994e65fb Mon Sep 17 00:00:00 2001 From: AadarshHegde123 <89495844+AadarshHegde123@users.noreply.github.com> Date: Tue, 25 Feb 2025 19:12:54 -0600 Subject: [PATCH 14/22] Update README.md --- GEMstack/offboard/calibration/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/README.md b/GEMstack/offboard/calibration/README.md index a5bf03ad8..db71bcca5 100644 --- a/GEMstack/offboard/calibration/README.md +++ b/GEMstack/offboard/calibration/README.md @@ -11,7 +11,7 @@ This repository contains tools for offline calibration of LiDAR and camera senso ## Calibration Pipeline -### 1. LiDAR-to-Vehicle Calibration (`make_gem_e4_ouster_v2.py`) +### 1. LiDAR-to-Vehicle Calibration (`lidar_to_vehicle.py`) **Method**: - **Ground Plane Detection**: 1. Crop LiDAR points near ground (Z ∈ [-3, -2]) From 28d728f4f5b085aa53427895c272642ef09ebe4c Mon Sep 17 00:00:00 2001 From: AadarshHegde123 <89495844+AadarshHegde123@users.noreply.github.com> Date: Tue, 25 Feb 2025 19:17:54 -0600 Subject: [PATCH 15/22] Update README.md --- GEMstack/offboard/calibration/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/GEMstack/offboard/calibration/README.md b/GEMstack/offboard/calibration/README.md index db71bcca5..0c58e6523 100644 --- a/GEMstack/offboard/calibration/README.md +++ b/GEMstack/offboard/calibration/README.md @@ -26,7 +26,7 @@ This repository contains tools for offline calibration of LiDAR and camera senso **Usage**: -python3 make_gem_e4_ouster_v2.py # Edit LiDAR data paths in script +python3 lidar_to_vehicle.py # Edit LiDAR data paths in script ### 2. CAMERA-to-Vehicle Calibration (`camera_to_vehicle_manual.py`) @@ -57,7 +57,8 @@ python3 lidar_to_camera.py # Ensure T_lidar_vehicle and T_camera_vehicle matri **3D Alignment Check**: 1. Use vis() function in scripts to view calibrated LiDAR/camera clouds - 2. Toggle VIS = True in make_gem_e4_ouster_v2.py for ground plane/object visualization + 2. Toggle VIS = True in lidar_to_vehicle.py for ground plane/object visualization + 3. Use test_transforms.py to visualize lidar point cloud on top of png image. Helps verify accuracy of lidar->camera. **Projection Validation**: 1. RGB image overlaid with transformed LiDAR points (Z-buffered) @@ -67,6 +68,7 @@ python3 lidar_to_camera.py # Ensure T_lidar_vehicle and T_camera_vehicle matri + ### Assumption and Notes 1. The sensor data should be time-aligned. From 81c5fb523dd3cedefe1d479f72e1f1276d4f99b3 Mon Sep 17 00:00:00 2001 From: rjsun-KA <92330153+rjsun06@users.noreply.github.com> Date: Wed, 26 Feb 2025 03:29:42 +0000 Subject: [PATCH 16/22] demo scripts --- .../calibration/camera_to_vehicle_manual.py | 17 +++++++++-------- ..._gem_e4_ouster_v2.py => lidar_to_vehicle.py} | 3 ++- 2 files changed, 11 insertions(+), 9 deletions(-) rename GEMstack/offboard/calibration/{make_gem_e4_ouster_v2.py => lidar_to_vehicle.py} (99%) diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index 3c61b93fd..dd414c9f8 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -5,9 +5,9 @@ import numpy as np N = 8 #how many point pairs you want to select -rgb_path = './data/color20.png' -depth_path = './data/depth20.tif' -lidar_path = './data/lidar20.npz' +rgb_path = './data/color32.png' +depth_path = './data/depth32.tif' +lidar_path = './data/lidar32.npz' img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) @@ -88,7 +88,7 @@ def crop(pc,ix=None,iy=None,iz=None): lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] lidar_post = crop(lidar_post,ix=(0,10),iy=(-5,5)) -vis(notebook=True).add_pc(lidar_post).show() +vis(notebook=False).add_pc(lidar_post).show() #%% def pick_n_img(img,n=4): @@ -190,12 +190,13 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): c2v = np.linalg.inv(v2c) print('camera->vehicle:',c2v) -v=vis(notebook=True) +v=vis(notebook=False) v.add_pc(lidar_post,color='blue') v.add_pc(np.pad(camera_points,((0,0),(0,1)),constant_values=1)@c2v.T[:,:3],color='red') v.show() -v=vis(notebook=True) -v.add_pc(np.pad(lidar_post,((0,0),(0,1)),constant_values=1)@v2c.T[:,:3],color='blue') +v=vis(notebook=False) +# v.add_pc(np.pad(lidar_post,((0,0),(0,1)),constant_values=1)@v2c.T[:,:3],color='blue') v.add_pc(camera_points,color='red') -v.show() \ No newline at end of file +v.show() +# %% diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/lidar_to_vehicle.py similarity index 99% rename from GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py rename to GEMstack/offboard/calibration/lidar_to_vehicle.py index bad0f1090..9e2deaebf 100644 --- a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py +++ b/GEMstack/offboard/calibration/lidar_to_vehicle.py @@ -6,6 +6,7 @@ import pyvista as pv import matplotlib.pyplot as plt VIS = False # True to show visuals +VIS = True # True to show visuals #%% things to extract tx,ty,tz,rx,ry,rz = [None] * 6 @@ -16,7 +17,7 @@ def vis(title='', ratio=1): print(title) pv.set_jupyter_backend('client') - plotter = pv.Plotter(notebook=True) + plotter = pv.Plotter(notebook=False) plotter.show_axes() class foo: def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): From 5e70824fad9a43398521cfc0fb08c52311fc1f76 Mon Sep 17 00:00:00 2001 From: whizkid42 Date: Tue, 25 Feb 2025 21:59:46 -0600 Subject: [PATCH 17/22] Updated date --- GEMstack/knowledge/calibration/gem_e4.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/knowledge/calibration/gem_e4.yaml b/GEMstack/knowledge/calibration/gem_e4.yaml index da971c253..d061ff018 100644 --- a/GEMstack/knowledge/calibration/gem_e4.yaml +++ b/GEMstack/knowledge/calibration/gem_e4.yaml @@ -1,4 +1,4 @@ -calibration_date: "2024-03-05" # Date of calibration YYYY-MM-DD +calibration_date: "2025-02-16" # Date of calibration YYYY-MM-DD reference: rear_axle_center # rear axle center rear_axle_height: 0.33 # height of rear axle center above flat ground gnss_location: [1.10,0.1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? From fb2bf76326b7df501f08703ba6621b5e2a6d21e0 Mon Sep 17 00:00:00 2001 From: aadarshhegde Date: Wed, 26 Feb 2025 00:13:08 -0600 Subject: [PATCH 18/22] fixing yaml matrix --- GEMstack/knowledge/calibration/gem_e4_oak.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/GEMstack/knowledge/calibration/gem_e4_oak.yaml b/GEMstack/knowledge/calibration/gem_e4_oak.yaml index b1b27186d..4f0d0cd53 100644 --- a/GEMstack/knowledge/calibration/gem_e4_oak.yaml +++ b/GEMstack/knowledge/calibration/gem_e4_oak.yaml @@ -1,5 +1,5 @@ reference: rear_axle_center # rear axle center -rotation: [[ 0.00349517, -0.03239524, 0.99946903], -[-0.99996547, 0.00742285, 0.0037375], -[-0.00753999, -0.99944757, -0.03236817]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated -center_position: [ 1.75864913, 0.01238124, 1.54408419] # meters, center camera, guesstimated +rotation: [[-0.025131, -0.0304479, 0.99922038], + [-0.99892897, 0.0396095, -0.0239167], + [-0.0388504, -0.99875123, -0.03141071]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated +center_position: [1.78251567,0.18649424,1.5399846] # meters, center camera, guesstimated \ No newline at end of file From 2e5bea57bb002ce7283bcfa9b82671a38bc38b26 Mon Sep 17 00:00:00 2001 From: aadarshhegde Date: Wed, 26 Feb 2025 13:17:11 -0600 Subject: [PATCH 19/22] code restructure + clean up --- .DS_Store | Bin 8196 -> 8196 bytes GEMstack/.DS_Store | Bin 6148 -> 8196 bytes .../calibration/camera_to_vehicle_manual.py | 37 ++------------- .../offboard/calibration/lidar_to_vehicle.py | 41 ++--------------- GEMstack/offboard/calibration/visualizer.py | 43 ++++++++++++++++++ 5 files changed, 52 insertions(+), 69 deletions(-) create mode 100644 GEMstack/offboard/calibration/visualizer.py diff --git a/.DS_Store b/.DS_Store index 043038f0b65f2f5ca0e0f558b8366b72ba550cd9..d93f745cc6d71dc794796241078e94b81ccd2e80 100644 GIT binary patch literal 8196 zcmeI1O>fgc5Qb;dG#`pUITR@uBum^-6dDyuD=r~T1QlOWgXjTJa1tvqb?sm$1QbQe z8U6|qSN;h67f$fb?zXa>h9g2{N7@~G_nlq)@vhTxiAXf3gE~=#hzcm|%OzA78258o zv5aonf)|J<3aLvT1vI2+MQs`k0YktLFa!(%L*QQ^fZuGk!kXWG)oW@)z!3N^3Gn;D zLSf&~eyO~Cb)b?d0DKnB(r}DCK*m_wH?&_WFGDe=${vIc6*|QbDjfY5%i$Z^FO^r} zBvd#FJ+sgmijcD-&XRHx4dqR32p9sZ1UPrULVaZI4%vzPy@niqbm#^Bo)`3>;*E75 zrS?l2j(FP-5H%q?rTe;0*PBISJ>Iw8*mI-F_Vy2v*~o6@@~~3=b!QaJIum;`jhc4& zlFyz8f!8}bwtFvJXVxum9R&Wwc6}$50q($o2h^q~APjcUpQQ9W#~<~VgyRZ_6wd;U!1EMqa|R!-aQd0z zRHBFU3|!lE41YwAsD|i4l5U2dR-A4QtKX(6`FIkk6`%Q@(&p(g9nt}2cuE~=Cr6h1 z<4aZ`0l3Blwo*z~cAYA8&~OU3d^ipLf~ zrH$)sNlF*c5zI6mQN}zDy#pUt9`V!gFsIv;^|LyhP{}VCh#1Jq9E*Alj zZFO2r+|${gyy@HHT-!o0L;I!jXn~mz0TP3$ J41vE&;5Wz}TkZe= literal 8196 zcmeHMziSjh6n=AVyaVBqN@62ZrV?XovxW}<{MOk*NuWdT1)J5sV0db(}fT`URx-8sD zA7hne=r7~(m^tFg3QJEMJ?hrADeag6icJ`Q;8 zT)yi%Jm76v-VWWRZMsi$vuE%>3iv0_K7+rK!*y(&zFXc0HF>2EXXORD<_|zgmuUh$ z3p#|RaiEE=y^3+JXZNJ=C0K)d)07kFAgd#Rjtswy=(@CCLqmaa{;$sqeukGm)Zrno z#?sjT96KLk|HGJ@zQj1^zy7A+57yxOZ_26Bu>u`K+#3s;g;$~b6Dkj)f%D|RCAXP%Q~z%_ zfB!#ETb8|u1L8pBfJ%Bhy)6v-+B$+=m9H_n>k*Te<3O}i(Ajbv(U#+g#UF+^*GM&q YsgJpf7(x5z9|A`5{>L~u8(1m9X!pg=D1l;Vg!5R7G!6k_$rNvH(MbRK$NPd1!5{#Xg6qcD< z9xou`oS#>cn3CV^cmauO*J=|pOC1Gc z3*%ZHg=$MPLmdTEL-X2NP7YCJee0n3?3~=Z{4SuYfq)Sh`e48drD0Sz$S0E%SlcHT zvkF?41sCPzAKj~1XzAXDJ-#cciP z@>h}MH-BamVcgiThf$CjC<+7;+(5z=l!SoEp%j?JGJ)x!7}dQ@SnS)$2mreoz{LOn delta 144 zcmZp1XfcprU|?W$DortDU=RQ@Ie-{Mvv5r;6q~50C<>Mbi{&t+GNdr1Goc|fox#d9M3a{835b<8-V}- diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index dd414c9f8..a28df6ae9 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -3,8 +3,11 @@ from scipy.spatial.transform import Rotation as R import matplotlib.pyplot as plt import numpy as np +from visualizer import visualizer N = 8 #how many point pairs you want to select + +# Update depending on where data is stored rgb_path = './data/color32.png' depth_path = './data/depth32.tif' lidar_path = './data/lidar32.npz' @@ -38,40 +41,8 @@ def vis(title='', ratio=1,notebook=False): print(title) pv.set_jupyter_backend('client') - plotter = pv.Plotter(notebook=notebook) - plotter.show_axes() - class foo: - def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): - plotter.camera.position = pos - plotter.camera.focal_point = foc - return self - def add_pc(self,pc,ratio=ratio,**kargs): - plotter.add_mesh( - pv.PolyData(pc*ratio), - render_points_as_spheres=True, - point_size=2, - **kargs) - return self - def add_line(self,p1,p2,ratio=ratio,**kargs): - plotter.add_mesh( - pv.Line(p1*ratio,p2*ratio), - **kargs, - line_width=1) - return self - def add_box(self,bound,trans,ratio=ratio): - l,w,h = map(lambda x:x*ratio,bound) - box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) - box = box.translate(list(map(lambda x:x*ratio,trans))) - plotter.add_mesh(box, color='yellow') - return self - def show(self): - plotter.show() - return self - def close(self): - plotter.close() - return None - return foo().set_cam() + return visualizer().set_cam() def crop(pc,ix=None,iy=None,iz=None): # crop a subrectangle in a pointcloud # usage: crop(pc, ix = (x_min,x_max), ...) diff --git a/GEMstack/offboard/calibration/lidar_to_vehicle.py b/GEMstack/offboard/calibration/lidar_to_vehicle.py index 9e2deaebf..31844ac31 100644 --- a/GEMstack/offboard/calibration/lidar_to_vehicle.py +++ b/GEMstack/offboard/calibration/lidar_to_vehicle.py @@ -5,6 +5,8 @@ from scipy.spatial.transform import Rotation as R import pyvista as pv import matplotlib.pyplot as plt +from visualizer import visualizer + VIS = False # True to show visuals VIS = True # True to show visuals @@ -17,40 +19,7 @@ def vis(title='', ratio=1): print(title) pv.set_jupyter_backend('client') - plotter = pv.Plotter(notebook=False) - plotter.show_axes() - class foo: - def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): - plotter.camera.position = pos - plotter.camera.focal_point = foc - return self - def add_pc(self,pc,ratio=ratio,**kargs): - plotter.add_mesh( - pv.PolyData(pc*ratio), - render_points_as_spheres=True, - point_size=2, - **kargs) - return self - def add_line(self,p1,p2,ratio=ratio,**kargs): - plotter.add_mesh( - pv.Line(p1*ratio,p2*ratio), - **kargs, - line_width=1) - return self - def add_box(self,bound,trans,ratio=ratio): - l,w,h = map(lambda x:x*ratio,bound) - box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) - box = box.translate(list(map(lambda x:x*ratio,trans))) - plotter.add_mesh(box, color='yellow') - return self - def show(self): - plotter.show() - return self - def close(self): - plotter.close() - return None - - return foo().set_cam() + return visualizer().set_cam() def load_scene(path): sc = np.load(path)['arr_0'] sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s @@ -78,7 +47,7 @@ def fit_plane_ransac(pc,tol=0.01): model.fit(pc[:,:-1], pc[:,-1]) a = model.estimator_.coef_ inter = model.estimator_.intercept_ - class foo: + class visual: def plot(self): inliers = pc[model.inlier_mask_] if pc.shape[1] == 2: @@ -95,7 +64,7 @@ def results(self): # return: array(D-1), float, array(N,3) # ^: , coeffs, intercept toward the plane, inliers of the fit return a,inter - return foo() + return visual() from scipy.spatial import cKDTree def pc_diff(pc0,pc1,tol=0.1): diff --git a/GEMstack/offboard/calibration/visualizer.py b/GEMstack/offboard/calibration/visualizer.py new file mode 100644 index 000000000..538172068 --- /dev/null +++ b/GEMstack/offboard/calibration/visualizer.py @@ -0,0 +1,43 @@ +import pyvista as pv + +class visualizer: + def __init__(self): + self.plotter = pv.Plotter(notebook=False) + self.plotter.show_axes() + + def set_cam(self, pos=(-20, 0, 20), foc=(0, 0, 0)): + self.plotter.camera.position = pos + self.plotter.camera.focal_point = foc + return self + + def add_pc(self, pc, ratio=1, **kargs): + self.plotter.add_mesh( + pv.PolyData(pc * ratio), + render_points_as_spheres=True, + point_size=2, + **kargs + ) + return self + + def add_line(self, p1, p2, ratio=1, **kargs): + self.plotter.add_mesh( + pv.Line(p1 * ratio, p2 * ratio), + **kargs, + line_width=1 + ) + return self + + def add_box(self, bound, trans, ratio=1): + l, w, h = map(lambda x: x * ratio, bound) + box = pv.Box(bounds=(-l/2, l/2, -w/2, w/2, -h/2, h/2)) + box = box.translate(list(map(lambda x: x * ratio, trans))) + self.plotter.add_mesh(box, color='yellow') + return self + + def show(self): + self.plotter.show() + return self + + def close(self): + self.plotter.close() + return None From 799c566e3b2c7d5c33c2cf2a400eb8284640b6cd Mon Sep 17 00:00:00 2001 From: aadarshhegde Date: Wed, 26 Feb 2025 13:18:33 -0600 Subject: [PATCH 20/22] removing conflict --- .DS_Store | Bin 8196 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 .DS_Store diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index d93f745cc6d71dc794796241078e94b81ccd2e80..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8196 zcmeI1O>fgc5Qb;dG#`pUITR@uBum^-6dDyuD=r~T1QlOWgXjTJa1tvqb?sm$1QbQe z8U6|qSN;h67f$fb?zXa>h9g2{N7@~G_nlq)@vhTxiAXf3gE~=#hzcm|%OzA78258o zv5aonf)|J<3aLvT1vI2+MQs`k0YktLFa!(%L*QQ^fZuGk!kXWG)oW@)z!3N^3Gn;D zLSf&~eyO~Cb)b?d0DKnB(r}DCK*m_wH?&_WFGDe=${vIc6*|QbDjfY5%i$Z^FO^r} zBvd#FJ+sgmijcD-&XRHx4dqR32p9sZ1UPrULVaZI4%vzPy@niqbm#^Bo)`3>;*E75 zrS?l2j(FP-5H%q?rTe;0*PBISJ>Iw8*mI-F_Vy2v*~o6@@~~3=b!QaJIum;`jhc4& zlFyz8f!8}bwtFvJXVxum9R&Wwc6}$50q($o2h^q~APjcUpQQ9W#~<~VgyRZ_6wd;U!1EMqa|R!-aQd0z zRHBFU3|!lE41YwAsD|i4l5U2dR-A4QtKX(6`FIkk6`%Q@(&p(g9nt}2cuE~=Cr6h1 z<4aZ`0l3Blwo*z~cAYA8&~OU3d^ipLf~ zrH$)sNlF*c5zI6mQN}zDy#pUt9`V!gFsIv;^|LyhP{}VCh#1Jq9E*Alj zZFO2r+|${gyy@HHT-!o0L;I!jXn~mz0TP3$ J41vE&;5Wz}TkZe= From 97f76c5757e23a3fe5cfa48d082bbb6274201706 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Wed, 26 Feb 2025 11:34:44 -0800 Subject: [PATCH 21/22] clean-up tested + working --- .../calibration/camera_to_vehicle_manual.py | 14 ++------------ GEMstack/offboard/calibration/lidar_to_vehicle.py | 12 +++++++----- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py index a28df6ae9..b88b22288 100644 --- a/GEMstack/offboard/calibration/camera_to_vehicle_manual.py +++ b/GEMstack/offboard/calibration/camera_to_vehicle_manual.py @@ -7,7 +7,7 @@ N = 8 #how many point pairs you want to select -# Update depending on where data is stored +# Update Depending on Where Data Stored rgb_path = './data/color32.png' depth_path = './data/depth32.tif' lidar_path = './data/lidar32.npz' @@ -59,7 +59,7 @@ def crop(pc,ix=None,iy=None,iz=None): lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] lidar_post = crop(lidar_post,ix=(0,10),iy=(-5,5)) -vis(notebook=False).add_pc(lidar_post).show() +# vis(notebook=False).add_pc(lidar_post).show() #%% def pick_n_img(img,n=4): @@ -161,13 +161,3 @@ def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): c2v = np.linalg.inv(v2c) print('camera->vehicle:',c2v) -v=vis(notebook=False) -v.add_pc(lidar_post,color='blue') -v.add_pc(np.pad(camera_points,((0,0),(0,1)),constant_values=1)@c2v.T[:,:3],color='red') -v.show() - -v=vis(notebook=False) -# v.add_pc(np.pad(lidar_post,((0,0),(0,1)),constant_values=1)@v2c.T[:,:3],color='blue') -v.add_pc(camera_points,color='red') -v.show() -# %% diff --git a/GEMstack/offboard/calibration/lidar_to_vehicle.py b/GEMstack/offboard/calibration/lidar_to_vehicle.py index 31844ac31..d82446e48 100644 --- a/GEMstack/offboard/calibration/lidar_to_vehicle.py +++ b/GEMstack/offboard/calibration/lidar_to_vehicle.py @@ -95,7 +95,8 @@ def pc_diff(pc0,pc1,tol=0.1): #================================================================ #%% load scene for ground plane -sc = load_scene('/mount/wp/GEMstack/data/lidar1.npz') +# Update depending on where data is stored +sc = load_scene('./data/lidar1.npz') # %% we crop to keep the ground cropped_sc = crop(sc,iz = (-3,-2)) @@ -133,9 +134,9 @@ def pc_diff(pc0,pc1,tol=0.1): rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() if False: # True to use the diff method to extract object. - # load data - sc0 = load_scene('/mount/wp/GEMstack/data/lidar70.npz') - sc1 = load_scene('/mount/wp/GEMstack/data/lidar78.npz') + # load data: update depending on where data is stored + sc0 = load_scene('./data/lidar70.npz') + sc1 = load_scene('./data/lidar78.npz') sc0 = sc0 @ rot.T + [0,0,tz] sc1 = sc1 @ rot.T + [0,0,tz] @@ -151,7 +152,8 @@ def pc_diff(pc0,pc1,tol=0.1): objects = pc_diff(cropped0,cropped1) else: #False to use only cropping - sc1 = load_scene('/mount/wp/GEMstack/data/lidar1.npz') + # Update depending on where data is stored + sc1 = load_scene('./data/lidar1.npz') objects = sc1 @ rot.T + [0,0,tz] From 16cd934c70f25e094c59f4715af77dc7c2e621f1 Mon Sep 17 00:00:00 2001 From: AadarshHegde123 Date: Wed, 26 Feb 2025 11:36:13 -0800 Subject: [PATCH 22/22] readme update --- GEMstack/offboard/calibration/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/GEMstack/offboard/calibration/README.md b/GEMstack/offboard/calibration/README.md index 0c58e6523..a7e3644b5 100644 --- a/GEMstack/offboard/calibration/README.md +++ b/GEMstack/offboard/calibration/README.md @@ -39,7 +39,8 @@ python3 lidar_to_vehicle.py # Edit LiDAR data paths in script 1. Get camera intrinsics: rosrun offboard\calibration\camera_info.py # Prints intrinsic matrix 2. Update camera_in in script with intrinsics - 3. Run calibration: + 3. Update data paths in script + 4. Run calibration: python3 camera_to_vehicle_manual.py