Unify source of camera geometry data
Due to parallel development, camera pose information ended up being
populated in two constants files. This makes //y2019/vision:constants
the source for all of the constants.
Also, because the camera 19 calibration was done as if it was a front
camera, I had to negate its x/y position and offset the angle by 180
degrees.
This commit also adds validation for //y2019/vision:constants.cc to
ensure that it is not accidentally changed in a way that the codegen
cannot account for.
Change-Id: I85654d973e15ae7bf76589be63c3d0eaf72c3a45
diff --git a/y2019/BUILD b/y2019/BUILD
index 3fae9e5..e3bac02 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -32,6 +32,7 @@
"//y2019/control_loops/superstructure/intake:intake_plants",
"//y2019/control_loops/superstructure/stilts:stilts_plants",
"//y2019/control_loops/superstructure/wrist:wrist_plants",
+ "//y2019/vision:constants",
],
)
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 915c53c..f84e945 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -16,6 +16,7 @@
#include "y2019/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/integral_stilts_plant.h"
#include "y2019/control_loops/superstructure/wrist/integral_wrist_plant.h"
+#include "y2019/vision/constants.h"
namespace y2019 {
namespace constants {
@@ -41,6 +42,24 @@
return deg * M_PI / 180.0;
}
+// Populates camera Pose information from the calibrated vision constants.
+void FillCameraPoses(
+ uint32_t teensy_processor_id,
+ ::std::array<Values::CameraCalibration, Values::kNumCameras> *cameras) {
+ ::std::array<int, 5> camera_ids =
+ vision::CameraSerialNumbers(teensy_processor_id);
+ for (size_t ii = 0; ii < camera_ids.size(); ++ii) {
+ const vision::CameraCalibration *calibration =
+ vision::GetCamera(camera_ids[ii]);
+ if (calibration != nullptr) {
+ vision::CameraGeometry geometry = calibration->geometry;
+ *cameras->at(ii).pose.mutable_pos() << geometry.location[0],
+ geometry.location[1], geometry.location[2];
+ cameras->at(ii).pose.set_theta(geometry.heading);
+ }
+ }
+}
+
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
Values::PotAndAbsConstants *const elevator = &r->elevator;
@@ -129,7 +148,12 @@
.nominal_skew_noise = 0.1,
.nominal_height_noise = 0.01};
- constexpr double kInchesToMeters = 0.0254;
+ // Deliberately make FOV a bit large so that we are overly conservative in
+ // our EKF.
+ for (auto &camera : r->cameras) {
+ camera.fov = M_PI_2 * 1.1;
+ }
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -144,11 +168,8 @@
stilts_params->zeroing_constants.measured_absolute_position = 0.0;
stilts->potentiometer_offset = 0.0;
- // Deliberately make FOV a bit large so that we are overly conservative in
- // our EKF.
- for (auto &camera : r->cameras) {
- camera.fov = M_PI_2 * 1.1;
- }
+ // For the simulation, just place cameras at the center of the robot with
+ // nominal angles (back/sides + 15 degree offset front cameras).
r->cameras[0].pose.set_theta(M_PI);
r->cameras[1].pose.set_theta(0.26);
r->cameras[2].pose.set_theta(-0.26);
@@ -194,23 +215,8 @@
stilts_params->zeroing_constants.measured_absolute_position = 0.0;
stilts->potentiometer_offset = 0.0;
- // Deliberately make FOV a bit large so that we are overly conservative in
- // our EKF.
- for (auto &camera : r->cameras) {
- camera.fov = M_PI_2 * 1.1;
- }
- // 0 - 2 ar ecurrently unpopulated:
- r->cameras[0].pose.set_theta(M_PI);
- r->cameras[1].pose.set_theta(0.26);
- r->cameras[2].pose.set_theta(-0.26);
- // 3 is front right
- r->cameras[3].pose.set_theta(-12.2377 / 180.0 * M_PI);
- *r->cameras[3].pose.mutable_pos() << 4.98126 * kInchesToMeters,
- 1.96988 * kInchesToMeters, 33.4276 * kInchesToMeters;
- // 4 is back
- r->cameras[4].pose.set_theta(M_PI + 2.581 * M_PI / 180.0);
- *r->cameras[4].pose.mutable_pos() << -6.93309 * kInchesToMeters,
- 2.64735 * kInchesToMeters, 32.8758 * kInchesToMeters;
+ FillCameraPoses(vision::CodeBotTeensyId(), &r->cameras);
+
break;
default:
diff --git a/y2019/constants.h b/y2019/constants.h
index 05f6380..5b00847 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -192,8 +192,6 @@
PotAndAbsConstants stilts;
struct CameraCalibration {
- // constants matrix to send to camera for calibration.
- Eigen::Matrix<float, 3, 4> for_camera;
// Pose of the camera relative to the robot.
::frc971::control_loops::TypedPose<double> pose;
// Field of view, in radians. This is total horizontal FOV, from left
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 982e04b..9ccc7d4 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -389,15 +389,6 @@
return r;
}
-std::array<int, 5> CameraSerialNumbers() {
- switch (ProcessorIdentifier()) {
- case 0xffff322e: // CODE bot
- return {{0, 0, 0, 16, 19}};
- default:
- return {{0, 0, 0, 0, 0}};
- }
-}
-
extern "C" {
void *__stack_chk_guard = (void *)0x67111971;
@@ -770,7 +761,8 @@
for (int i = 0; i < 5; ++i) {
const y2019::vision::CameraCalibration *const constants =
- y2019::vision::GetCamera(CameraSerialNumbers()[i]);
+ y2019::vision::GetCamera(y2019::vision::CameraSerialNumbers(
+ ProcessorIdentifier())[i]);
(void)constants;
calibration.calibration(0, 0) = constants->intrinsics.mount_angle;
calibration.calibration(0, 1) = constants->intrinsics.focal_length;
@@ -807,8 +799,9 @@
case 'c':
printf("This UART board is 0x%" PRIx32 "\n", ProcessorIdentifier());
for (int i = 0; i < 5; ++i) {
- printf("Camera slot %d's serial number is %d\n", i,
- CameraSerialNumbers()[i]);
+ printf(
+ "Camera slot %d's serial number is %d\n", i,
+ y2019::vision::CameraSerialNumbers(ProcessorIdentifier())[i]);
}
break;
case 'v':
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 2bdfd9c..90c7dff 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -12,12 +12,41 @@
cc_library(
name = "constants",
- srcs = ["constants.cc"],
+ srcs = [
+ "constants.cc",
+ "constants_formatting.cc",
+ ],
hdrs = ["constants.h"],
compatible_with = mcu_cpus,
visibility = ["//visibility:public"],
)
+cc_binary(
+ name = "constants_formatting",
+ srcs = ["constants_formatting_main.cc"],
+ deps = [":constants"],
+)
+
+genrule(
+ name = "generate_constants",
+ outs = ["validate_constants.cc"],
+ cmd = "$(location :constants_formatting) $(OUTS)",
+ tools = [":constants_formatting"],
+)
+
+sh_test(
+ name = "constants_formatting_test",
+ srcs = ["constants_formatting_test.sh"],
+ args = [
+ "$(location :constants.cc)",
+ "$(location :validate_constants.cc)",
+ ],
+ data = [
+ ":constants.cc",
+ ":validate_constants.cc",
+ ],
+)
+
cc_library(
name = "target_finder",
srcs = [
@@ -104,7 +133,6 @@
cc_binary(
name = "global_calibration",
srcs = [
- "constants_formatting.cc",
"global_calibration.cc",
],
restricted_to = VISION_TARGETS,
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index bc8a56d..e41a53b 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -82,9 +82,9 @@
-0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,
},
{
- {{6.93309 * kInchesToMeters, -2.64735 * kInchesToMeters,
+ {{-6.93309 * kInchesToMeters, 2.64735 * kInchesToMeters,
32.8758 * kInchesToMeters}},
- 2.58102 / 180.0 * M_PI,
+ -177.419 / 180.0 * M_PI,
},
{
19,
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index cc6775e..3f07f87 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -79,7 +79,26 @@
const CameraCalibration *GetCamera(int camera_id);
-void DumpCameraConstants(int camera_id, const CameraCalibration &value);
+// Serial number of the teensy for each robot.
+constexpr uint32_t CodeBotTeensyId() { return 0xffff322e; }
+
+// Get the IDs of the cameras in each port for a particular teensy board.
+// inlined so that we don't have to deal with including it in the autogenerated
+// constants.cc.
+inline std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
+ switch (processor_id) {
+ case CodeBotTeensyId():
+ return {{0, 0, 0, 16, 19}};
+ default:
+ return {{0, 0, 0, 0, 0}};
+ }
+}
+
+// Rewrites constants.cc, adding camera calibration constants for the camera_id
+// specified. If camera_id is less than zero, just rewrites the file without
+// changing anything.
+void DumpCameraConstants(const char *fname, int camera_id,
+ const CameraCalibration &value);
} // namespace vision
} // namespace y2019
diff --git a/y2019/vision/constants_formatting.cc b/y2019/vision/constants_formatting.cc
index 272246e..a2922a0 100644
--- a/y2019/vision/constants_formatting.cc
+++ b/y2019/vision/constants_formatting.cc
@@ -51,8 +51,9 @@
<< "\"" << filename_prefix << "\",\n " << num_images << ",\n }";
}
-void DumpCameraConstants(int camera_id, const CameraCalibration &value) {
- std::ofstream o("y2019/vision/constants.cc");
+void DumpCameraConstants(const char *fname, int camera_id,
+ const CameraCalibration &value) {
+ std::ofstream o(fname);
o << R"(#include "y2019/vision/constants.h"
namespace y2019 {
diff --git a/y2019/vision/constants_formatting_main.cc b/y2019/vision/constants_formatting_main.cc
new file mode 100644
index 0000000..fd0bb22
--- /dev/null
+++ b/y2019/vision/constants_formatting_main.cc
@@ -0,0 +1,11 @@
+#include "y2019/vision/constants.h"
+#include <iostream>
+
+int main(int argc, char *argv[]) {
+ if (argc != 2) {
+ ::std::cout << "Expected a command line argument specifying the file name "
+ "to write to.\n";
+ return 1;
+ }
+ ::y2019::vision::DumpCameraConstants(argv[1], -1, {});
+}
diff --git a/y2019/vision/constants_formatting_test.sh b/y2019/vision/constants_formatting_test.sh
new file mode 100755
index 0000000..a74a988
--- /dev/null
+++ b/y2019/vision/constants_formatting_test.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+# The constants file should not have changed in a way that will not be preserved
+# when we rerun the codegen.
+diff $1 $2
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index 94048ad..186e025 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -247,7 +247,7 @@
results.dataset = info;
results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
results.geometry = CameraGeometry::get(&geometry[0]);
- DumpCameraConstants(info.camera_id, results);
+ DumpCameraConstants("y2019/vision/constants.cc", info.camera_id, results);
}
} // namespace y2019