Improve localizer update debugging
* Only send LocalizerDebug messages when something interesting happens.
* On debug field, draw both the implied robot position of the update as
well as the camera position.
* Indicate accepted vs. rejected images via red/green colors.
* Leave old corrections on the display for 2 seconds and gradually fade
them out.
* Add aggregate statistics about why we are rejecting image corrections
to the readouts table.
Change-Id: Ibc3397cb5654aacbc6cce4e5f3eb71f0371692cc
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index edec5c2..b12cdc3 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,4 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
@@ -64,6 +64,13 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "localizer_debug_ts_fbs",
+ srcs = ["localizer_debug.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "localizer",
srcs = ["localizer.cc"],
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 937d8c4..650a0c5 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -2,6 +2,10 @@
#include "y2020/constants.h"
+DEFINE_bool(send_empty_debug, false,
+ "If true, send LocalizerDebug messages on every tick, even if "
+ "they would be empty.");
+
namespace y2020 {
namespace control_loops {
namespace drivetrain {
@@ -92,6 +96,7 @@
event_loop_
->MakeSender<y2020::control_loops::drivetrain::LocalizerDebug>(
"/drivetrain")) {
+ statistics_.rejection_counts.fill(0);
// TODO(james): The down estimator has trouble handling situations where the
// robot is constantly wiggling but not actually moving much, and can cause
// drift when using accelerometer readings.
@@ -176,11 +181,26 @@
}
}
}
- const auto vector_offset =
- builder.fbb()->CreateVector(debug_offsets.data(), debug_offsets.size());
- LocalizerDebug::Builder debug_builder = builder.MakeBuilder<LocalizerDebug>();
- debug_builder.add_matches(vector_offset);
- CHECK(builder.Send(debug_builder.Finish()));
+ if (FLAGS_send_empty_debug || !debug_offsets.empty()) {
+ const auto vector_offset =
+ builder.fbb()->CreateVector(debug_offsets.data(), debug_offsets.size());
+ const auto rejections_offset =
+ builder.fbb()->CreateVector(statistics_.rejection_counts.data(),
+ statistics_.rejection_counts.size());
+
+ CumulativeStatistics::Builder stats_builder =
+ builder.MakeBuilder<CumulativeStatistics>();
+ stats_builder.add_total_accepted(statistics_.total_accepted);
+ stats_builder.add_total_candidates(statistics_.total_candidates);
+ stats_builder.add_rejection_reason_count(rejections_offset);
+ const auto stats_offset = stats_builder.Finish();
+
+ LocalizerDebug::Builder debug_builder =
+ builder.MakeBuilder<LocalizerDebug>();
+ debug_builder.add_matches(vector_offset);
+ debug_builder.add_statistics(stats_offset);
+ CHECK(builder.Send(debug_builder.Finish()));
+ }
}
aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5>
@@ -209,14 +229,10 @@
<< aos::time::DurationInSeconds(monotonic_offset)
<< " when at time of " << now << " and capture time estimate of "
<< capture_time;
+ std::optional<RejectionReason> rejection_reason;
if (capture_time > now) {
LOG(WARNING) << "Got camera frame from the future.";
- ImageMatchDebug::Builder builder(*fbb);
- builder.add_camera(camera_index);
- builder.add_accepted(false);
- builder.add_rejection_reason(RejectionReason::IMAGE_FROM_FUTURE);
- debug_offsets.push_back(builder.Finish());
- return debug_offsets;
+ rejection_reason = RejectionReason::IMAGE_FROM_FUTURE;
}
if (!result.has_camera_calibration()) {
LOG(WARNING) << "Got camera frame without calibration data.";
@@ -225,6 +241,8 @@
builder.add_accepted(false);
builder.add_rejection_reason(RejectionReason::NO_CALIBRATION);
debug_offsets.push_back(builder.Finish());
+ statistics_.rejection_counts[static_cast<size_t>(
+ RejectionReason::NO_CALIBRATION)]++;
return debug_offsets;
}
// Per the ImageMatchResult specification, we can actually determine whether
@@ -238,12 +256,7 @@
// seems reasonable, but may be unnecessarily low or high.
constexpr float kMaxTurretVelocity = 1.0;
if (is_turret && std::abs(turret_data.velocity) > kMaxTurretVelocity) {
- ImageMatchDebug::Builder builder(*fbb);
- builder.add_camera(camera_index);
- builder.add_accepted(false);
- builder.add_rejection_reason(RejectionReason::TURRET_TOO_FAST);
- debug_offsets.push_back(builder.Finish());
- return debug_offsets;
+ rejection_reason = RejectionReason::TURRET_TOO_FAST;
}
CHECK(result.camera_calibration()->has_fixed_extrinsics());
const Eigen::Matrix<float, 4, 4> fixed_extrinsics =
@@ -266,12 +279,15 @@
builder.add_accepted(false);
builder.add_rejection_reason(RejectionReason::NO_RESULTS);
debug_offsets.push_back(builder.Finish());
+ statistics_
+ .rejection_counts[static_cast<size_t>(RejectionReason::NO_RESULTS)]++;
return debug_offsets;
}
int index = -1;
for (const frc971::vision::sift::CameraPose *vision_result :
*result.camera_poses()) {
+ ++statistics_.total_candidates;
++index;
ImageMatchDebug::Builder builder(*fbb);
@@ -286,6 +302,8 @@
!vision_result->has_field_to_target()) {
builder.add_accepted(false);
builder.add_rejection_reason(RejectionReason::NO_TRANSFORMS);
+ statistics_.rejection_counts[static_cast<size_t>(
+ RejectionReason::NO_TRANSFORMS)]++;
debug_offsets.push_back(builder.Finish());
continue;
}
@@ -294,11 +312,21 @@
const Eigen::Matrix<float, 4, 4> H_field_target =
FlatbufferToTransformationMatrix(*vision_result->field_to_target());
+ const Eigen::Matrix<float, 4, 4> H_field_camera =
+ H_field_target * H_camera_target.inverse();
// Back out the robot position that is implied by the current camera
// reading. Note that the Pose object ignores any roll/pitch components, so
// if the camera's extrinsics for pitch/roll are off, this should just
// ignore it.
- const Pose measured_camera_pose(H_field_target * H_camera_target.inverse());
+ const Pose measured_camera_pose(H_field_camera);
+ builder.add_camera_x(measured_camera_pose.rel_pos().x());
+ builder.add_camera_y(measured_camera_pose.rel_pos().y());
+ // Because the camera uses Z as forwards rather than X, just calculate the
+ // debugging theta value using the transformation matrix directly (note that
+ // the rest of this file deliberately does not care what convention the
+ // camera uses, since that is encoded in the extrinsics themselves).
+ builder.add_camera_theta(
+ std::atan2(H_field_camera(1, 2), H_field_camera(0, 2)));
// Calculate the camera-to-robot transformation matrix ignoring the
// pitch/roll of the camera.
// TODO(james): This could probably be made a bit more efficient, but I
@@ -313,12 +341,30 @@
const Eigen::Matrix<float, 3, 1> Z(measured_pose.rel_pos().x(),
measured_pose.rel_pos().y(),
measured_pose.rel_theta());
+ builder.add_implied_robot_x(Z(0));
+ builder.add_implied_robot_y(Z(1));
+ builder.add_implied_robot_theta(Z(2));
// Pose of the target in the robot frame.
// Note that we use measured_pose's transformation matrix rather than just
// doing H_robot_camera * H_camera_target because measured_pose ignores
// pitch/roll.
Pose pose_robot_target(measured_pose.AsTransformationMatrix().inverse() *
H_field_target);
+
+ // Turret is zero when pointed backwards.
+ builder.add_implied_turret_goal(
+ aos::math::NormalizeAngle(M_PI + pose_robot_target.heading()));
+
+ // Since we've now built up all the information that is useful to include in
+ // the debug message, bail if we have reason to do so.
+ if (rejection_reason) {
+ builder.add_rejection_reason(*rejection_reason);
+ statistics_.rejection_counts[static_cast<size_t>(*rejection_reason)]++;
+ builder.add_accepted(false);
+ debug_offsets.push_back(builder.Finish());
+ continue;
+ }
+
// TODO(james): Figure out how to properly handle calculating the
// noise. Currently, the values are deliberately tuned so that image updates
// will not be trusted overly much. In theory, we should probably also be
@@ -357,6 +403,8 @@
AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
builder.add_accepted(false);
builder.add_rejection_reason(RejectionReason::HIGH_THETA_DIFFERENCE);
+ statistics_.rejection_counts[static_cast<size_t>(
+ RejectionReason::HIGH_THETA_DIFFERENCE)]++;
debug_offsets.push_back(builder.Finish());
continue;
}
@@ -371,18 +419,12 @@
AOS_LOG(WARNING, "Dropped image match due to age of image.\n");
builder.add_accepted(false);
builder.add_rejection_reason(RejectionReason::IMAGE_TOO_OLD);
+ statistics_.rejection_counts[static_cast<size_t>(
+ RejectionReason::IMAGE_TOO_OLD)]++;
debug_offsets.push_back(builder.Finish());
continue;
}
- builder.add_implied_robot_x(Z(0));
- builder.add_implied_robot_y(Z(1));
- builder.add_implied_robot_theta(Z(2));
-
- // Turret is zero when pointed backwards.
- builder.add_implied_turret_goal(
- aos::math::NormalizeAngle(M_PI + pose_robot_target.heading()));
-
std::optional<RejectionReason> correction_rejection;
const Input U = ekf_.MostRecentInput();
// For the correction step, instead of passing in the measurement directly,
@@ -436,8 +478,11 @@
if (correction_rejection) {
builder.add_accepted(false);
builder.add_rejection_reason(*correction_rejection);
+ statistics_
+ .rejection_counts[static_cast<size_t>(*correction_rejection)]++;
} else {
builder.add_accepted(true);
+ statistics_.total_accepted++;
}
debug_offsets.push_back(builder.Finish());
}
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 19ee4ad..5458797 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -78,6 +78,23 @@
double velocity = 0.0; // rad/sec
};
+ static constexpr size_t kNumRejectionReasons =
+ static_cast<int>(RejectionReason::MAX) -
+ static_cast<int>(RejectionReason::MIN) + 1;
+
+ struct Statistics {
+ int total_accepted = 0;
+ int total_candidates = 0;
+ static_assert(0 == static_cast<int>(RejectionReason::MIN));
+ static_assert(
+ kNumRejectionReasons ==
+ sizeof(
+ std::invoke_result<decltype(EnumValuesRejectionReason)>::type) /
+ sizeof(RejectionReason),
+ "RejectionReason has non-contiguous error values.");
+ std::array<int, kNumRejectionReasons> rejection_counts;
+ };
+
// Processes new image data from the given pi and updates the EKF.
aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> HandleImageMatch(
size_t camera_index, std::string_view pi,
@@ -111,6 +128,8 @@
// Target selector to allow us to satisfy the LocalizerInterface requirements.
frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+
+ Statistics statistics_;
};
} // namespace drivetrain
diff --git a/y2020/control_loops/drivetrain/localizer_debug.fbs b/y2020/control_loops/drivetrain/localizer_debug.fbs
index 89aea68..e94d251 100644
--- a/y2020/control_loops/drivetrain/localizer_debug.fbs
+++ b/y2020/control_loops/drivetrain/localizer_debug.fbs
@@ -17,6 +17,9 @@
pose_index: uint8 (id: 1);
local_image_capture_time_ns:long (id: 2);
roborio_image_capture_time_ns:long (id: 3);
+ camera_x:float (id: 11);
+ camera_y:float (id: 12);
+ camera_theta:float (id: 13);
implied_robot_x:float (id: 4);
implied_robot_y:float (id: 5);
implied_robot_theta:float (id: 6);
@@ -27,8 +30,16 @@
image_age_sec:float (id: 10);
}
+table CumulativeStatistics {
+ total_accepted:int (id: 0);
+ total_candidates:int (id: 1);
+ // Indexed by integer value of RejectionReason enum.
+ rejection_reason_count:[int] (id: 2);
+}
+
table LocalizerDebug {
matches:[ImageMatchDebug] (id: 0);
+ statistics:CumulativeStatistics (id: 1);
}
root_type LocalizerDebug;