Calibration Pipeline

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.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?
 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,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
+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]

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
+position: [1.10, 0.03773583, 1.95320223] # meters, calibrated by Hang's watchful eye
+rotation: [[0.99939639, 0.02547917, 0.023615], 
+           [-0.02530848, 0.99965156, -0.00749882],
+           [-0.02379784, 0.00689664, 0.999693]]  #rotation matrix mapping lidar frame to vehicle frame

GEMstack/knowledge/calibration/gem_e4_rl.yaml

@@ -0,0 +1,5 @@
+position: [0.0898392124024201, 0.71876803481624, 1.71024199833245]
+reference: rear_axle_center
+rotation: [[0.6847850928670124, 0.19803293816635642, -0.7013218462220591], [0.728026894383745,
+    -0.14318896257364072, 0.6704280439025839], [0.03234528777238433, -0.9696802959730001,
+    -0.2422269719924619]]

GEMstack/knowledge/calibration/gem_e4_rl_in.yaml

@@ -0,0 +1,5 @@
+center: [953.302889408274, 608.1966398872765]
+distort: [-0.2522996862206216, 0.12482113115174773, -0.0005993692936397102, -0.00017949453391219192,
+  -0.03499498178003368]
+focal: [1181.6177321982138, 1180.0783789769903]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_rr.yaml

@@ -0,0 +1,5 @@
+position: [0.11419591502518789, -0.6896311735924415, 1.711181163333824]
+reference: rear_axle_center
+rotation: [[-0.7359657309159472, 0.15986191414426415, -0.6578743127098735], [0.6768157805459531,
+    0.14993386619459964, -0.7207220233709469], [-0.016578363047300385, -0.9756864271752846,
+    -0.21854325362408236]]

GEMstack/knowledge/calibration/gem_e4_rr_in.yaml

@@ -0,0 +1,5 @@
+center: [956.2663906909728, 569.2039945552984]
+distort: [-0.25040910859151444, 0.1109210921906881, -0.00041247665414900384, 0.0008205455176671751,
+  -0.026395952816984845]
+focal: [1162.3787554048329, 1162.855381183851]
+skew: 0

GEMstack/knowledge/calibration/make_camera_lidar_yaml.py

@@ -0,0 +1,48 @@
+import yaml
+import numpy as np
+from calib_util import load_ex, load_in
+
+#  
+# THIS FILE SHOULD BE RUN FROM ITS LOCAL DIRECTORY FOR THE PATHS TO WORK
+#
+
+# Destination files name
+output_file = 'gem_e4_perception_cameras.yaml'
+
+# Collect names of all sensors and associated extrinsic/intrinsic files
+camera_files = {'front': ['gem_e4_oak.yaml', 'gem_e4_oak_in.yaml'],
+                'front_right': ['gem_e4_fr.yaml', 'gem_e4_oak_in.yaml'],
+                'front_left': ['gem_e4_fl.yaml', 'gem_e4_fl_in.yaml'],
+                'back_right': ['gem_e4_rr.yaml', 'gem_e4_rr_in.yaml'],
+                'back_left': ['gem_e4_rl.yaml', 'gem_e4_rl_in.yaml']}
+lidar_file = 'gem_e4_ouster.yaml'
+
+# Initialize variables
+output_dict = {'cameras': {}}
+T_lidar_to_vehicle = load_ex(lidar_file, 'matrix')
+
+# Collect data for all cameras
+for camera in camera_files:
+    # Load from files
+    ex_file = camera_files[camera][0]
+    in_file = camera_files[camera][1]
+    T_camera_to_vehicle = load_ex(ex_file, 'matrix')
+    K, D = load_in(in_file, 'matrix', return_distort=True)
+
+    # Calculate necessary values
+    T_lidar_to_camera = np.linalg.inv(T_camera_to_vehicle) @ T_lidar_to_vehicle
+
+    # Store in the proper format
+    camera_dict = {}
+    camera_dict['K'] = K
+    camera_dict['D'] = D
+    camera_dict['T_l2c'] = T_lidar_to_camera
+    camera_dict['T_l2v'] = T_lidar_to_vehicle
+    for key in camera_dict:
+        if type(camera_dict[key]) == np.ndarray:
+            camera_dict[key] = camera_dict[key].tolist()
+    output_dict['cameras'][camera] = camera_dict
+
+# Write to file
+with open(output_file,'w') as stream:
+    yaml.safe_dump(output_dict, stream)

GEMstack/offboard/calibration/.gitignore

@@ -0,0 +1,2 @@
+__pycache__
+