Add 7971 front/back camera constants and fix height reference
The height reference for the targets is actually supposed to be from the
camera, not the floor.
Change-Id: Iee5a23230b4b1fbc6f0f1ebaf5344135cc78b14a
diff --git a/y2019/constants.cc b/y2019/constants.cc
index aebc1fe..d467876 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -129,6 +129,7 @@
.nominal_skew_noise = 0.1,
.nominal_height_noise = 0.01};
+ constexpr double kInchesToMeters = 0.0254;
switch (team) {
// A set of constants for tests.
case 1:
@@ -192,6 +193,24 @@
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;
break;
default:
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 8015772..04e4157 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -108,7 +108,7 @@
Scalar heading; // radians
// The distance from the camera to the target.
Scalar distance; // meters
- // Height of the target from the floor.
+ // Height of the target from the camera.
Scalar height; // meters
// The angle of the target relative to the frame of the camera.
Scalar skew; // radians
@@ -292,7 +292,7 @@
view.reading.heading = heading;
view.reading.distance = distance;
view.reading.skew = skew;
- view.reading.height = target.pose().abs_pos().z();
+ view.reading.height = relative_pose.rel_pos().z();
view.target = ⌖
view.camera_pose = camera_abs_pose;
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 1f10dc0..229af58 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -66,7 +66,7 @@
// center of the camera's image plane to the center of the target.
float distance;
- // Height of the target in meters. Specifically, the distance from the floor
+ // Height of the target in meters. Specifically, the distance from the camera
// to the center of the target.
float height;
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index 6238cb1..fa7c130 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -59,6 +59,8 @@
55,
}};
+// Note: x/y should be negated and heading should be offset by 180 deg to
+// account for this being calibrated on the rear of the robot.
CameraCalibration camera_19 = {
{
-0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,