Cleaning up calibration files
Removing intrinsic-only files and replacing with all extrinsics
Have dummy 9971 extrinsics based on 971 extrinsics until we calibrate 9971
Changed naming of calibration data structure in calibrate_extrinsics from intrinsics
to the more general caliibration, and made it use extrinsic as initial condition,
when it exists.
Change-Id: I59813bde7f4372a5c210f82cfe94e577eb4df93a
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index 6d5cbf9..c7c467d 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -1,16 +1,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_from971_2023-02-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json' %}
}
],
"target_map": {% include 'y2023/vision/maps/target_map.json' %}
diff --git a/y2023/vision/calib_files/calibration_pi-2023-base-calib.json b/y2023/vision/calib_files/calibration_pi-2023-base-calib.json
deleted file mode 100755
index d75e5ca..0000000
--- a/y2023/vision/calib_files/calibration_pi-2023-base-calib.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 7971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-00"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_2013-01-18_09-15-19.438113048.json b/y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_2013-01-18_09-15-19.438113048.json
deleted file mode 100755
index c3ecd9e..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_2013-01-18_09-15-19.438113048.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 7971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-01"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_2013-01-18_09-02-49.498335208.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_2013-01-18_09-02-49.498335208.json
deleted file mode 100755
index 2e6f2a0..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_2013-01-18_09-02-49.498335208.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi2",
- "team_number": 7971,
- "intrinsics": [
- 895.543945,
- 0.0,
- 645.250122,
- 0.0,
- 895.308838,
- 354.297241,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.455658,
- 0.272167,
- 0.000796,
- -0.000206,
- -0.0975
- ],
- "calibration_timestamp": 1358499769498335208,
- "camera_id": "23-02"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_2013-01-18_09-39-27.378322133.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_2013-01-18_09-39-27.378322133.json
deleted file mode 100755
index d9c4154..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_2013-01-18_09-39-27.378322133.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi3",
- "team_number": 7971,
- "intrinsics": [
- 892.089172,
- 0.0,
- 648.780701,
- 0.0,
- 892.362854,
- 342.340668,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.451178,
- 0.258187,
- 0.001071,
- 0.000017,
- -0.085526
- ],
- "calibration_timestamp": 1358501967378322133,
- "camera_id": "23-03"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_2013-01-18_09-39-42.039874176.json b/y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_2013-01-18_09-39-42.039874176.json
deleted file mode 100755
index aa541b5..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_2013-01-18_09-39-42.039874176.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi4",
- "team_number": 7971,
- "intrinsics": [
- 890.071899,
- 0.0,
- 620.69519,
- 0.0,
- 890.307434,
- 365.158844,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.449088,
- 0.25594,
- 0.000415,
- 0.000142,
- -0.084656
- ],
- "calibration_timestamp": 1358501982039874176,
- "camera_id": "23-04"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
deleted file mode 100755
index c9d960a..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-01_2013-01-18_08-56-17.194305339.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 971,
- "intrinsics": [
- 913.133728,
- 0.0,
- 575.592285,
- 0.0,
- 917.686462,
- 364.544403,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.454162,
- 0.255307,
- -0.000166,
- 0.008949,
- -0.079813
- ],
- "calibration_timestamp": 1358499377194305339,
- "camera_id": "23-01"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_2013-01-18_09-38-22.915096335.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_2013-01-18_09-38-22.915096335.json
deleted file mode 100755
index 3afee2e..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-05_2013-01-18_09-38-22.915096335.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 971,
- "intrinsics": [
- 890.980713,
- 0.0,
- 619.298645,
- 0.0,
- 890.668762,
- 364.009766,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.449172,
- 0.252318,
- 0.000881,
- -0.000615,
- -0.082208
- ],
- "calibration_timestamp": 1358501902915096335,
- "camera_id": "23-05"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-99_000000.json b/y2023/vision/calib_files/calibration_pi-971-1_cam-23-99_000000.json
deleted file mode 100644
index 4b41658..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-1_cam-23-99_000000.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-99"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_2013-01-18_09-32-06.409252911.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_2013-01-18_09-32-06.409252911.json
deleted file mode 100755
index 47f1d30..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_2013-01-18_09-32-06.409252911.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi2",
- "team_number": 971,
- "intrinsics": [
- 893.242981,
- 0.0,
- 639.796692,
- 0.0,
- 892.498718,
- 354.109344,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.451751,
- 0.252422,
- 0.000531,
- 0.000079,
- -0.079369
- ],
- "calibration_timestamp": 1358501526409252911,
- "camera_id": "23-06"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
deleted file mode 100755
index 306bea1..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-11_2013-01-18_10-01-30.177115986.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi3",
- "team_number": 971,
- "intrinsics": [
- 891.026001,
- 0.0,
- 620.086731,
- 0.0,
- 890.566895,
- 385.035126,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.448299,
- 0.250123,
- -0.00042,
- -0.000127,
- -0.078433
- ],
- "calibration_timestamp": 1358503290177115986,
- "camera_id": "23-11"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-99_000000.json b/y2023/vision/calib_files/calibration_pi-971-2_cam-23-99_000000.json
deleted file mode 100644
index 9257aee..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-2_cam-23-99_000000.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi2",
- "team_number": 971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-99"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_2013-01-18_09-11-56.768663953.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_2013-01-18_09-11-56.768663953.json
deleted file mode 100755
index fc78ef8..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_2013-01-18_09-11-56.768663953.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi3",
- "team_number": 971,
- "intrinsics": [
- 892.627869,
- 0.0,
- 629.289978,
- 0.0,
- 891.73761,
- 373.299896,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.454901,
- 0.266778,
- -0.000316,
- -0.000469,
- -0.091357
- ],
- "calibration_timestamp": 1358500316768663953,
- "camera_id": "23-07"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
deleted file mode 100755
index 393beef..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-10_2013-01-18_10-02-40.377380613.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi2",
- "team_number": 971,
- "intrinsics": [
- 894.002502,
- 0.0,
- 636.431335,
- 0.0,
- 893.723816,
- 377.069672,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.446659,
- 0.244189,
- 0.000632,
- 0.000171,
- -0.074849
- ],
- "calibration_timestamp": 1358503360377380613,
- "camera_id": "23-10"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-99_000000.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-99_000000.json
deleted file mode 100644
index 03b2687..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-99_000000.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi3",
- "team_number": 971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-99"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json
deleted file mode 100755
index 5b92f75..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_2013-01-18_09-27-45.150551614.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi4",
- "team_number": 971,
- "intrinsics": [
- 891.88385,
- 0.0,
- 642.268616,
- 0.0,
- 890.626465,
- 353.272919,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.448426,
- 0.246817,
- 0.000002,
- 0.000948,
- -0.076717
- ],
- "calibration_timestamp": 1358501265150551614,
- "camera_id": "23-08"
-}
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
deleted file mode 100755
index e119c43..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-09_2013-01-18_09-02-59.650270322.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 971,
- "intrinsics": [
- 893.617798,
- 0.0,
- 612.44397,
- 0.0,
- 893.193115,
- 375.196381,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.443805,
- 0.238734,
- 0.000133,
- 0.000448,
- -0.071068
- ],
- "calibration_timestamp": 1358499779650270322,
- "camera_id": "23-09"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-99_000000.json b/y2023/vision/calib_files/calibration_pi-971-4_cam-23-99_000000.json
deleted file mode 100644
index 4a0b632..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-4_cam-23-99_000000.json
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- "node_name": "pi4",
- "team_number": 971,
- "intrinsics": [
- 893.759521,
- 0.0,
- 645.470764,
- 0.0,
- 893.222351,
- 388.150269,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.44902,
- 0.248409,
- -0.000537,
- -0.000112,
- -0.076989
- ],
- "calibration_timestamp": 1358500519438113048,
- "camera_id": "23-99"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json
new file mode 100644
index 0000000..b6e0def
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi1",
+ "team_number": 9971,
+ "intrinsics": [
+ 893.617798,
+ 0.0,
+ 612.44397,
+ 0.0,
+ 893.193115,
+ 375.196381,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.443805,
+ 0.238734,
+ 0.000133,
+ 0.000448,
+ -0.071068
+ ],
+ "fixed_extrinsics": { "data": [ -0.487722, 0.222354, 0.844207, 0.025116, 0.864934, -0.008067, 0.501821, -0.246003, 0.118392, 0.974933, -0.188387, 0.532497, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358499779650270322,
+ "camera_id": "23-09"
+}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_from971_2023-02-23.json
new file mode 100644
index 0000000..6293443
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_from971_2023-02-23.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi2",
+ "team_number": 9971,
+ "intrinsics": [
+ 894.002502,
+ 0.0,
+ 636.431335,
+ 0.0,
+ 893.723816,
+ 377.069672,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.446659,
+ 0.244189,
+ 0.000632,
+ 0.000171,
+ -0.074849
+ ],
+ "fixed_extrinsics": { "data": [ 0.852213, 0.227336, 0.471224, 0.220072, 0.485092, -0.005909, -0.874443, -0.175232, -0.196008, 0.973799, -0.115315, 0.61409, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358503360377380613,
+ "camera_id": "23-10"
+}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json
new file mode 100644
index 0000000..ec4c69c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi3",
+ "team_number": 9971,
+ "intrinsics": [
+ 891.026001,
+ 0.0,
+ 620.086731,
+ 0.0,
+ 890.566895,
+ 385.035126,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.448299,
+ 0.250123,
+ -0.00042,
+ -0.000127,
+ -0.078433
+ ],
+ "fixed_extrinsics": { "data": [ 0.492232, -0.163335, -0.855002, 0.020122, -0.866067, 0.006706, -0.499883, -0.174518, 0.087382, 0.986548, -0.138158, 0.645307, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358503290177115986,
+ "camera_id": "23-11"
+}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json
new file mode 100644
index 0000000..3d0f566
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi4",
+ "team_number": 9971,
+ "intrinsics": [
+ 891.127197,
+ 0.0,
+ 640.291321,
+ 0.0,
+ 891.176453,
+ 359.578705,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.452948,
+ 0.262567,
+ 0.00088,
+ -0.000253,
+ -0.089368
+ ],
+ "fixed_extrinsics": { "data": [ -0.865915, -0.186983, -0.463928, -0.014873, -0.473362, 0.006652, 0.880843, -0.215738, -0.161617, 0.982341, -0.094271, 0.676433, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358499579812698894,
+ "camera_id": "23-12"
+}
diff --git a/y2023/vision/calibrate_extrinsics.cc b/y2023/vision/calibrate_extrinsics.cc
index 10e1639..7e650a3 100644
--- a/y2023/vision/calibrate_extrinsics.cc
+++ b/y2023/vision/calibrate_extrinsics.cc
@@ -1,5 +1,6 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+
#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
#include "aos/events/logging/log_writer.h"
@@ -15,6 +16,8 @@
#include "y2023/constants/constants_generated.h"
#include "y2023/vision/vision_util.h"
+#include <opencv2/core/eigen.hpp>
+
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_string(target_type, "charuco_diamond",
@@ -22,10 +25,12 @@
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
DEFINE_string(output_logs, "/tmp/calibration/",
"Output folder for visualization logs.");
-DEFINE_string(base_intrinsics, "",
- "Intrinsics to use for extrinsics calibration.");
+DEFINE_string(base_calibration, "",
+ "Intrinsics (and optionally extrinsics) to use for extrinsics "
+ "calibration.");
DEFINE_string(output_calibration, "",
- "Output json file to use for the resulting extrinsics.");
+ "Output json file to use for the resulting calibration "
+ "(intrinsics and extrinsics).");
namespace frc971 {
namespace vision {
@@ -51,7 +56,7 @@
TargetType target_type = TargetTypeFromString(FLAGS_target_type);
std::unique_ptr<aos::EventLoop> constants_event_loop =
factory->MakeEventLoop("constants_fetcher", pi_event_loop->node());
- if (FLAGS_base_intrinsics.empty()) {
+ if (FLAGS_base_calibration.empty()) {
frc971::constants::ConstantsFetcher<y2023::Constants> constants_fetcher(
constants_event_loop.get());
*intrinsics =
@@ -60,7 +65,7 @@
pi_event_loop->node()->name()->string_view()));
} else {
*intrinsics = aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
- FLAGS_base_intrinsics);
+ FLAGS_base_calibration);
}
extractor = std::make_unique<Calibration>(
factory, pi_event_loop.get(), imu_event_loop.get(), FLAGS_pi,
@@ -75,7 +80,7 @@
CHECK(pi_number);
const std::string pi_name = absl::StrCat("pi", *pi_number);
- aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> intrinsics =
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> calibration =
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>::Empty();
{
// Now, accumulate all the data into the data object.
@@ -102,7 +107,8 @@
const aos::Node *const pi_node =
aos::configuration::GetNode(factory.configuration(), pi_name);
- CalibrationAccumulatorState accumulator_state(&factory, &data, &intrinsics);
+ CalibrationAccumulatorState accumulator_state(&factory, &data,
+ &calibration);
reader.OnStart(imu_node, [&accumulator_state, imu_node, &factory]() {
accumulator_state.imu_event_loop =
@@ -152,6 +158,24 @@
// Set y value to -1 m (approx distance from imu to the board/world)
nominal_initial_state(1, 0) = 1.0;
+ // If available, pull extrinsics from the calibration file
+ const calibration::CameraCalibration *calib_msg = &calibration.message();
+ if (calib_msg->has_fixed_extrinsics()) {
+ LOG(INFO) << "Using extrinsiscs from calibration file as initial condition";
+ const cv::Mat extrinsics_cv(
+ 4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ calib_msg->fixed_extrinsics()->data()->data())));
+ CHECK_EQ(extrinsics_cv.total(),
+ calib_msg->fixed_extrinsics()->data()->size());
+ Eigen::Matrix4d ext_mat;
+ cv::cv2eigen(extrinsics_cv, ext_mat);
+ Eigen::Affine3d extrinsics(ext_mat);
+ Eigen::Affine3d H_camera_imu = extrinsics.inverse();
+ nominal_pivot_to_camera = H_camera_imu.rotation();
+ nominal_pivot_to_camera_translation = H_camera_imu.translation();
+ }
+
CalibrationParameters calibration_parameters;
calibration_parameters.initial_orientation = nominal_initial_orientation;
calibration_parameters.pivot_to_camera = nominal_pivot_to_camera;
@@ -198,10 +222,10 @@
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
solved_extrinsics = Solve(data, &calibration_parameters);
- intrinsics.mutable_message()->clear_fixed_extrinsics();
- intrinsics.mutable_message()->clear_turret_extrinsics();
+ calibration.mutable_message()->clear_fixed_extrinsics();
+ calibration.mutable_message()->clear_turret_extrinsics();
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
- merged_calibration = aos::MergeFlatBuffers(&intrinsics.message(),
+ merged_calibration = aos::MergeFlatBuffers(&calibration.message(),
&solved_extrinsics.message());
if (!FLAGS_output_calibration.empty()) {