Fixing broken calib file and changing to only load calibrated data
We were sending a default calibration file for every possible pi/robot combo.
Instead, let's just send only the ones that we have calibrated.
I've included a default / reference calib file that can be copied
if we aren't able (or don't want) to do a calibration.
Change-Id: I23237078331d548e5a21ffeb2ba0057a5523c67d
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
index a144853..39c7911 100644
--- a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
@@ -1,6 +1,6 @@
{
"node_name": "pi1",
- "team_number": 7971,
+ "team_number": 971,
"intrinsics": [
392.276093,
0.0,
diff --git a/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
new file mode 100644
index 0000000..8c9472e
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "reference",
+ "team_number": 1971,
+ "intrinsics": [
+ 388.0,
+ 0.0,
+ 320.0,
+ 0.0,
+ 388.0,
+ 240.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "calibration_timestamp": 1597994992500905688
+}
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 33eb1a9..6172ae7 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -7,7 +7,7 @@
import define_training_data as dtd
-glog.setLevel("WARN")
+glog.setLevel("INFO")
class CameraIntrinsics:
@@ -36,24 +36,14 @@
def load_camera_definitions():
### CAMERA DEFINITIONS
+ # We only load in cameras that have a calibration file
+ # These are stored in y2020/vision/tools/python_code/calib_files
+ # See reference_calibration_pi_cam.json for an example default file
+ #
+ # Or better yet, use //y2020/vision:calibration to calibrate the camera
+ # using a Charuco target board
- # Robot camera has:
- # FOV_H = 93.*math.pi()/180.
- # FOV_V = 70.*math.pi()/180.
-
- # Create fake camera (based on USB webcam params)
- fx = 810.
- fy = 810.
- cx = 320.
- cy = 240.
-
- # Define a web_cam
- web_cam_int = CameraIntrinsics()
- web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy],
- [0, 0, 1]])
- web_cam_int.dist_coeffs = np.zeros((5, 1))
-
- web_cam_ext = CameraExtrinsics()
+ # Extrinsic definition
# Camera rotation from robot x,y,z to opencv (z, -x, -y)
# This is extrinsics for the turret camera
# camera pose relative to center, base of the turret
@@ -63,37 +53,30 @@
[[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(
+ turret_cam_ext = CameraExtrinsics()
+ turret_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])
+ turret_cam_ext.T = np.array([15.0 * 0.0254, 15.0 * 0.0254, 41.0 * 0.0254])
+ default_cam_ext = CameraExtrinsics()
+ default_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ default_cam_ext.T = np.array([0.0, 0.0, 0.0])
- web_cam_params = CameraParameters()
- web_cam_params.camera_int = web_cam_int
- # 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
+ default_cam_params = CameraParameters()
+ # Currently, all cameras have this same set of extrinsics
+ default_cam_params.camera_ext = default_cam_ext
+ default_cam_params.turret_ext = turret_cam_ext
camera_list = []
- # TODO<Jim>: Should probably make this a dict to make replacing easier
- for team_number in (971, 7971, 8971, 9971):
- for node_name in ("pi0", "pi1", "pi2", "pi3", "pi4", "pi5"):
- camera_base = copy.deepcopy(web_cam_params)
- camera_base.node_name = node_name
- camera_base.team_number = team_number
- camera_list.append(camera_base)
-
dir_name = dtd.bazel_name_fix('calib_files')
+ glog.debug("Searching for calibration files in " + dir_name)
for filename in os.listdir(dir_name):
glog.debug("Inspecting %s", filename)
- if ("cam-calib-int" in filename or 'calibration' in filename) and filename.endswith(".json"):
+ 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())
@@ -104,6 +87,8 @@
# JSON.
# See which one we have and parse accordingly.
if 'hostname' in calib_dict:
+ glog.warn("WARNING: Using older calibration file.")
+ glog.warn("Preferred usage is y2020/vision:calibration")
hostname_split = calib_dict["hostname"].split("-")
team_number = int(hostname_split[1])
node_name = hostname_split[0] + hostname_split[2]
@@ -117,14 +102,13 @@
dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape(
(1, 5))
- # 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))
- camera_calib.camera_int.camera_matrix = copy.copy(
- camera_matrix)
- camera_calib.camera_int.dist_coeffs = copy.copy(
- dist_coeffs)
+ glog.info("Found calib for " + node_name + ", team #" +
+ str(team_number))
+ camera_base = copy.deepcopy(default_cam_params)
+ camera_base.node_name = node_name
+ camera_base.team_number = team_number
+ camera_base.camera_int.camera_matrix = copy.copy(camera_matrix)
+ camera_base.camera_int.dist_coeffs = copy.copy(dist_coeffs)
+ camera_list.append(camera_base)
return camera_list