Add turret extrinsics for cameras
Currently, the extrinsics are shared among all the pi's, but
once we have some fixed-position pi's we can deal with it.
Change-Id: If29c05bca03a346371eeaf44892dea156b195d9a
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index a55cb23..53464ac 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -67,6 +67,7 @@
main = "load_sift_training.py",
srcs_version = "PY2AND3",
deps = [
+ ":load_sift_training",
"//external:python-glog",
"//y2020/vision/sift:sift_fbs_python",
"@bazel_tools//tools/python/runfiles",
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index da20a52..b8c71ff 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -9,6 +9,7 @@
glog.setLevel("WARN")
+
class CameraIntrinsics:
def __init__(self):
self.camera_matrix = []
@@ -27,6 +28,7 @@
def __init__(self):
self.camera_int = CameraIntrinsics()
self.camera_ext = CameraExtrinsics()
+ self.turret_ext = None
self.node_name = ""
self.team_number = -1
@@ -60,14 +62,22 @@
[[np.cos(camera_pitch), 0.0, -np.sin(camera_pitch)], [0.0, 1.0, 0.0],
[np.sin(camera_pitch), 0.0,
np.cos(camera_pitch)]])
- web_cam_ext.R = np.array(camera_pitch_matrix * np.matrix(
- [[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
- # Tape measure calibration-- need to pull from CAD or automate this
+ web_cam_ext.R = np.array(
+ camera_pitch_matrix *
+ np.matrix([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
+ #web_cam_ext.T = np.array([0., 0., 0.])
web_cam_ext.T = np.array([2.0 * 0.0254, -6.0 * 0.0254, 41.0 * 0.0254])
+ fixed_ext = CameraExtrinsics()
+ fixed_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ fixed_ext.T = np.array([0.0, 0.0, 0.0])
web_cam_params = CameraParameters()
web_cam_params.camera_int = web_cam_int
- web_cam_params.camera_ext = web_cam_ext
+ # Fixed extrinsics are for the turret, which is centered on the robot and
+ # pointed straight backwards.
+ web_cam_params.camera_ext = fixed_ext
+ web_cam_params.turret_ext = web_cam_ext
camera_list = []
@@ -98,8 +108,8 @@
# Look for match, and replace camera_intrinsics
for camera_calib in camera_list:
if camera_calib.node_name == node_name and camera_calib.team_number == team_number:
- glog.info("Found calib for %s, team #%d" % (node_name,
- team_number))
+ glog.info("Found calib for %s, team #%d" %
+ (node_name, team_number))
camera_calib.camera_int.camera_matrix = copy.copy(
camera_matrix)
camera_calib.camera_int.dist_coeffs = copy.copy(
diff --git a/y2020/vision/tools/python_code/camera_definition_test.py b/y2020/vision/tools/python_code/camera_definition_test.py
index 732dbb8..f157ba3 100644
--- a/y2020/vision/tools/python_code/camera_definition_test.py
+++ b/y2020/vision/tools/python_code/camera_definition_test.py
@@ -2,26 +2,9 @@
import math
import numpy as np
-
-class CameraIntrinsics:
- def __init__(self):
- self.camera_matrix = []
- self.dist_coeffs = []
-
-
-class CameraExtrinsics:
- def __init__(self):
- self.R = []
- self.T = []
-
-
-class CameraParameters:
- def __init__(self):
- self.camera_int = CameraIntrinsics()
- self.camera_ext = CameraExtrinsics()
- self.node_name = ""
- self.team_number = -1
-
+from y2020.vision.tools.python_code.camera_definition import (CameraIntrinsics,
+ CameraExtrinsics,
+ CameraParameters)
### CAMERA DEFINITIONS
@@ -56,6 +39,6 @@
camera_base = copy.deepcopy(web_cam_params)
camera_base.node_name = node_name
camera_base.team_number = team_number
- camera_base.camera_ext.T = np.asarray(
- np.float32([i + 1, i + 1, i + 1]))
+ camera_base.camera_ext.T = np.asarray(np.float32([i + 1, i + 1,
+ i + 1]))
camera_list.append(camera_base)
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
index 5ca574d..a21a06f 100644
--- a/y2020/vision/tools/python_code/load_sift_training.py
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -27,6 +27,19 @@
return output_list
+def list_to_transformation_matrix(values, fbb):
+ """Puts a list of values into an FBB TransformationMatrix."""
+
+ TransformationMatrix.TransformationMatrixStartDataVector(fbb, len(values))
+ for n in reversed(values):
+ fbb.PrependFloat32(n)
+ list_offset = fbb.EndVector(len(values))
+
+ TransformationMatrix.TransformationMatrixStart(fbb)
+ TransformationMatrix.TransformationMatrixAddData(fbb, list_offset)
+ return TransformationMatrix.TransformationMatrixEnd(fbb)
+
+
def main():
target_data_list = None
@@ -138,18 +151,15 @@
for camera_calib in camera_calib_list:
fixed_extrinsics_list = rot_and_trans_to_list(
camera_calib.camera_ext.R, camera_calib.camera_ext.T)
- TransformationMatrix.TransformationMatrixStartDataVector(
- fbb, len(fixed_extrinsics_list))
- for n in reversed(fixed_extrinsics_list):
- fbb.PrependFloat32(n)
- fixed_extrinsics_data_offset = fbb.EndVector(
- len(fixed_extrinsics_list))
+ fixed_extrinsics_vector = list_to_transformation_matrix(
+ fixed_extrinsics_list, fbb)
- TransformationMatrix.TransformationMatrixStart(fbb)
- TransformationMatrix.TransformationMatrixAddData(
- fbb, fixed_extrinsics_data_offset)
- fixed_extrinsics_vector = TransformationMatrix.TransformationMatrixEnd(
- fbb)
+ turret_extrinsics_vector = None
+ if camera_calib.turret_ext is not None:
+ turret_extrinsics_list = rot_and_trans_to_list(
+ camera_calib.turret_ext.R, camera_calib.turret_ext.T)
+ turret_extrinsics_vector = list_to_transformation_matrix(
+ turret_extrinsics_list, fbb)
# TODO: Need to add in distortion coefficients here
# For now, just send camera paramter matrix (fx, fy, cx, cy)
@@ -179,6 +189,9 @@
fbb, dist_coeffs_vector)
CameraCalibration.CameraCalibrationAddFixedExtrinsics(
fbb, fixed_extrinsics_vector)
+ if turret_extrinsics_vector is not None:
+ CameraCalibration.CameraCalibrationAddTurretExtrinsics(
+ fbb, turret_extrinsics_vector)
camera_calibration_vector.append(
CameraCalibration.CameraCalibrationEnd(fbb))