merge s2025 into s2025_perception_merge_Tianyu

Author: lukasdumasius

.github/workflows/python-app.yml

@@ -30,7 +30,7 @@ jobs:
     - name: Lint with flake8
       run: |
         # stop the build if there are Python syntax errors or undefined names
-        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1
+        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --ignore=F824 --show-source --statistics --exclude=__init__.py || exit 1
         # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations)
         # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1
         # if we want to enable documentation checks, uncomment the line below

.gitignore

@@ -22,6 +22,7 @@ downloads/
 eggs/
 .eggs/
 lib/
+!frontend/*
 lib64/
 parts/
 sdist/
@@ -166,8 +167,11 @@ cython_debug/
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
-#.idea/
 
+.idea/
+
+# ZED run files
+**/*.run
 .vscode/
 setup/zed_sdk.run
 
@@ -182,3 +186,14 @@ cuda/
 /5.9
 /parking_data
 /perception_4.9
+#Ignore ROS bags
+*.bag
+
+cuda/
+homework/yolov8n.pt
+homework/yolo11n.pt
+yolov8n.pt
+yolo11n.pt
+
+# Computation  Graph   of Launch File Outputs
+launch_visualization/graph

GEMstack/knowledge/calibration/calib_util.py

@@ -0,0 +1,106 @@
+#%%
+import yaml
+from yaml import SafeDumper
+import numpy as np
+import cv2
+def represent_flow_style_list(dumper, data):
+    return dumper.represent_sequence(yaml.resolver.BaseResolver.DEFAULT_SEQUENCE_TAG, data, flow_style=True)
+SafeDumper.add_representer(list, represent_flow_style_list)
+#%%
+class FlowListDumper(yaml.Dumper):
+    def represent_list(self, data):
+        return self.represent_sequence('tag:yaml.org,2002:seq', data, flow_style=True)
+
+def load_ex(path,mode,ref='rear_axle_center'):
+    with open(path) as stream:
+        y = yaml.safe_load(stream)
+    assert y['reference'] == ref
+    if mode == 'matrix':
+        ret = np.eye(4)
+        ret[0:3,0:3] = y['rotation']
+        ret[:-1,3] = y['position']
+        return ret
+    elif mode == 'tuple':
+        return np.array(y['rotation']),np.array(y['position'])
+
+
+def save_ex(path,rotation=None,translation=None,matrix=None,ref='rear_axle_center'):
+    if matrix is not None:
+        rot = matrix[0:3,0:3]
+        trans = matrix[0:3,3]
+        save_ex(path,rot,trans,ref=ref)
+        return
+    ret = {}
+    ret['reference'] = ref
+    ret['rotation'] = rotation
+    ret['position'] = translation
+    for i in ret:
+        if type(ret[i]) == np.ndarray:
+            ret[i] = ret[i].tolist()
+    print(yaml.dump(ret,Dumper=SafeDumper,default_flow_style=False))
+    with open(path,'w') as stream:
+        yaml.dump(ret,stream,Dumper=SafeDumper,default_flow_style=False)
+
+def load_in(path,mode='matrix',return_distort=False):
+    with open(path) as stream:
+        y = yaml.safe_load(stream)
+    if 'skew' not in y: y['skew'] = 0
+    if 'distort' not in y: y['distort'] = [0,0,0,0,0]
+    if mode == 'matrix':
+        ret = np.zeros((3,3))
+        ret[0,0],ret[1,1] = y['focal']
+        ret[2,2] = 1.
+        ret[0:2,2] = y['center']
+        ret[0,1] = y['skew']
+        if return_distort:
+            return ret,np.array(y['distort'])
+        else:
+            return ret
+    elif mode == 'tuple':
+        return {'focal':np.array(y['focal']),
+                'center':np.array(y['center']),
+                'skew':np.array(y['skew']),
+                'distort':np.array(y['distort'])}
+
+from collections.abc import Iterable
+def save_in(path,focal=None,center=None,skew=0,distort=[0.0]*5,matrix=None):
+    if matrix is not None:
+        focal = matrix.diagonal()[0:2]
+        skew = matrix[0,1]
+        center = matrix[0:2,2]
+        save_in(path,focal,center,skew,distort)
+        return
+    ret = {}
+    ret['focal'] = focal
+    ret['center'] = center
+    ret['skew'] = skew 
+    assert len(distort) in [4,5]
+    ret['distort'] = distort
+    if len(ret['distort']) == 4:
+        ret['distort'] = list(ret['distort'])+[0.0]
+    for i in ret:
+        if type(ret[i]) == np.ndarray:
+            ret[i] = ret[i].tolist()
+        if isinstance(ret[i],Iterable):
+            ret[i] = [*map(float,ret[i])]
+    print(yaml.dump(ret,Dumper=SafeDumper,default_flow_style=False))
+    with open(path,'w') as stream:
+        yaml.dump(ret,stream,Dumper=SafeDumper,default_flow_style=False)
+
+def undistort_image(image, camera_matrix, distortion_coefficients):
+    h,  w = image.shape[:2]
+    newK, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, distortion_coefficients, (w,h), 1, (w,h))
+    image = cv2.undistort(image, camera_matrix, distortion_coefficients, None, newK)
+    return image, newK
+
+
+#%%
+if __name__ == "__main__":
+    #%%
+    rot, trans = load_ex('/mnt/GEMstack/GEMstack/knowledge/calibration/gem_e4_ouster.yaml',mode='tuple')
+    save_ex('/tmp/test.yaml',rot,trans)
+    #%%
+    focal = [1,2,3]
+    center = [400,500]
+    save_in('/tmp/test.yaml',focal,center)
+    load_in('/tmp/test.yaml',mode='tuple')

GEMstack/knowledge/calibration/gem_e4.yaml

@@ -4,4 +4,18 @@ rear_axle_height: 0.2794      # 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?
 gnss_yaw: 0.0 # radians
 top_lidar: !include "gem_e4_ouster.yaml"
-front_camera: !include "gem_e4_oak.yaml"
+front_camera:
+  extrinsics: !include "gem_e4_oak.yaml"
+  intrinsics: !include "gem_e4_oak_in.yaml"
+front_right_camera:
+  extrinsics: !include "gem_e4_fr.yaml"
+  intrinsics: !include "gem_e4_fr_in.yaml"
+front_left_camera:
+  extrinsics: !include "gem_e4_fl.yaml"
+  intrinsics: !include "gem_e4_fl_in.yaml"
+rear_right_camera:
+  extrinsics: !include "gem_e4_rr.yaml"
+  intrinsics: !include "gem_e4_rr_in.yaml"
+rear_left_camera:
+  extrinsics: !include "gem_e4_rl.yaml"
+  intrinsics: !include "gem_e4_rl_in.yaml"

GEMstack/knowledge/calibration/gem_e4_fl.yaml

@@ -0,0 +1,5 @@
+position: [2.008491967178938, 0.9574436688609637, 1.7222845229507735]
+reference: rear_axle_center
+rotation: [[0.7229102844527417, -0.13938889438297952, 0.6767358840457229], [-0.6904150547378912,
+    -0.18396833469067211, 0.6996304053015531], [0.026977264941612008, -0.9729986577348562,
+    -0.22922879230680995]]

GEMstack/knowledge/calibration/gem_e4_fl_in.yaml

@@ -0,0 +1,5 @@
+center: [971.5122150421694, 601.7847095069886]
+distort: [-0.2625420437513607, 0.1425651774165483, -0.0004946279626072071, -0.00033457504102070386,
+  -0.042732740327368145]
+focal: [1183.2337731693713, 1182.3831532373445]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_fr.yaml

@@ -0,0 +1,5 @@
+position: [1.8861563355156226, -0.7733611068168774, 1.6793040225335112]
+reference: rear_axle_center
+rotation: [[-0.7168464770690616, -0.10046018208578958, 0.6899557088168523], [-0.6970911725372957,
+    0.12308618950445319, -0.7063382243117325], [-0.01396515249660048, -0.9872981017750231,
+    -0.15826380744561577]]

GEMstack/knowledge/calibration/gem_e4_fr_in.yaml

@@ -0,0 +1,5 @@
+center: [966.4326452411585, 608.5803255934914]
+distort: [-0.2701363254469883, 0.16439325523243875, -0.001607207824773341, -7.412467081891699e-05,
+  -0.06199397580030171]
+focal: [1176.2554468073797, 1175.1456876174707]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_oak.yaml

@@ -1,8 +1,5 @@
-# Calibration for front rgb camera->vehicle on GEM e4
-# Calibration Date: 02/25/2025 05:09
-
-reference: rear_axle_center  # rear axle center
-rotation: [[ 0.00349517, -0.03239524, 0.99946903], # rotation component of camera -> vehicle matrix
-          [-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] # translation component of camera -> vehicle matrix (in meters)
+position: [1.8680678362969751, 0.03483728869549903, 1.6545932338230158]
+reference: rear_axle_center
+rotation: [[0.020064651878799838, -0.013111205776054045, 0.99971271174677], [-0.9997929081548379,
+    0.0031358785499412617, 0.020107388418472497], [-0.003398609756043868, -0.9999091271454714,
+    -0.013045570240818732]]

GEMstack/knowledge/calibration/gem_e4_oak_in.yaml

@@ -0,0 +1,2 @@
+center: [573.37109375, 363.700927734375]
+focal: [684.8333129882812, 684.6096801757812]