Add camera calibrations for box of pis to 2022
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ie4fd11ac69d7be9ba889db070ce1bd372b0f2d69
diff --git a/y2022/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json b/y2022/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
new file mode 100755
index 0000000..52f6bc0
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi1",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.369812,
+ 0.0,
+ 292.325653,
+ 0.0,
+ 388.513733,
+ 224.371063,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.126935,
+ -0.218447,
+ -0.000152,
+ 0.001158,
+ 0.06266
+ ],
+ "calibration_timestamp": 1623537339636386620
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json b/y2022/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
new file mode 100755
index 0000000..c502e35
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.7565,
+ 0.0,
+ 285.024506,
+ 0.0,
+ 388.915039,
+ 222.227539,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128415,
+ -0.212528,
+ 0.001165,
+ 0.000579,
+ 0.054853
+ ],
+ "calibration_timestamp": 1623537020325393444
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json b/y2022/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
new file mode 100755
index 0000000..52069c3
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi3",
+ "team_number": 7971,
+ "intrinsics": [
+ 389.35611,
+ 0.0,
+ 339.345673,
+ 0.0,
+ 389.516235,
+ 240.247787,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.122511,
+ -0.209383,
+ -0.001212,
+ 0.000041,
+ 0.05674
+ ],
+ "calibration_timestamp": 1623537211977365877
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json b/y2022/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
new file mode 100755
index 0000000..3cc765f
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "pi4",
+ "team_number": 7971,
+ "intrinsics": [
+ 390.301514,
+ 0.0,
+ 356.104095,
+ 0.0,
+ 389.884491,
+ 231.157303,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128595,
+ -0.229324,
+ -0.001145,
+ 0.001602,
+ 0.079774
+ ],
+ "calibration_timestamp": 1623537445706564865
+}
\ No newline at end of file
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 3b34ca2..2f8a127 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -55,7 +55,8 @@
self.timestamp = 0
-def compute_extrinsic(camera_pitch, camera_yaw, T_camera, is_turret):
+def compute_extrinsic(camera_roll, camera_pitch, camera_yaw, T_camera,
+ is_turret):
# Compute the extrinsic calibration based on pitch and translation
# Includes camera rotation from robot x,y,z to opencv (z, -x, -y)
@@ -65,6 +66,11 @@
base_cam_ext = CameraExtrinsics()
turret_cam_ext = CameraExtrinsics()
+ camera_roll_matrix = np.array(
+ [[1.0, 0.0, 0.0], [0.0, np.cos(camera_roll), -np.sin(camera_roll)],
+ [0.0, np.sin(camera_roll),
+ np.cos(camera_roll)]])
+
camera_pitch_matrix = np.array(
[[np.cos(camera_pitch), 0.0,
np.sin(camera_pitch)], [0.0, 1.0, 0.0],
@@ -83,10 +89,10 @@
base_cam_ext.R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
base_cam_ext.T = np.array([0.0, 0.0, 0.0])
- turret_cam_ext.R = camera_yaw_matrix @ camera_pitch_matrix @ robot_to_camera_rotation
+ turret_cam_ext.R = camera_yaw_matrix @ camera_pitch_matrix @ camera_roll_matrix @ robot_to_camera_rotation
turret_cam_ext.T = T_camera
else:
- base_cam_ext.R = camera_yaw_matrix @ camera_pitch_matrix @ robot_to_camera_rotation
+ base_cam_ext.R = camera_yaw_matrix @ camera_pitch_matrix @ camera_roll_matrix @ robot_to_camera_rotation
base_cam_ext.T = T_camera
turret_cam_ext = None
@@ -95,6 +101,7 @@
def compute_extrinsic_by_pi(pi_number, team_number):
# Defaults for all cameras
+ camera_roll = 0.0
camera_pitch = -35.0 * np.pi / 180.0
camera_yaw = 0.0
is_turret = True
@@ -127,10 +134,26 @@
elif pi_number == "pi4":
camera_yaw = -90.0 * np.pi / 180.0
T = np.array([-10.5 * 0.0254, -5.0 * 0.0254, 28.5 * 0.0254])
+ elif team_number == 7971:
+ # Cameras are flipped upside down
+ camera_roll = 180.0 * np.pi / 180.0
+ if pi_number == "pi1":
+ camera_yaw = 90.0 * np.pi / 180.0
+ T = np.array([-6.0 * 0.0254, -8.0 * 0.0254, 0.5 * 0.0254])
+ elif pi_number == "pi2":
+ camera_yaw = 0.0
+ T = np.array([3.75 * 0.0254, 7.5 * 0.0254, 0.5 * 0.0254])
+ elif pi_number == "pi3":
+ camera_yaw = 0.0
+ T = np.array([3.75 * 0.0254, -4.25 * 0.0254, 0.5 * 0.0254])
+ elif pi_number == "pi4":
+ camera_yaw = 0.0
+ T = np.array([-6.0 * 0.0254, -7.0 * 0.0254, 0.5 * 0.0254])
else:
glog.fatal("Unknown team number for extrinsics")
- return compute_extrinsic(camera_pitch, camera_yaw, T, is_turret)
+ return compute_extrinsic(camera_roll, camera_pitch, camera_yaw, T,
+ is_turret)
def load_camera_definitions():
@@ -153,7 +176,6 @@
glog.debug("Inspecting %s", filename)
if ("cam-calib-int" in filename
or 'calibration' in filename) and filename.endswith(".json"):
-
# Extract intrinsics from file
calib_file = open(dir_name + "/" + filename, 'r')
calib_dict = json.loads(calib_file.read())