Sundry extrinsics/target mapping fixes.
Notes:
* Rate-limit foxglove image converter.
* Don't log foxglove image channels.
* Make pose error more permissive to allow for better matches covering
gaps in field.
* Allow matching between more than just the immediately previous target
detection for situations where multiple cameras see multiple separate
targets.
* I mucked around with --outlier_std_devs; idk how much impact it has.
* Fix event loops & distributed clock calculation in extrinsics.
* Update box of orins calibration appropriately.
* Fix FindCameraCalibration call in y2024 target_mapper.
* Generate a misnamed map of the blue-side [6, 10] tags on the EPA field.
The variations from the nominal seem to largely reflect real variation
in the field.
* Bump up max distance to consider targets in target mapper; this seems
to have negligible impact.
Change-Id: I6b790568908beaba2bee5006c9e4fd58c5cc4e47
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index f5c8dab..d1aa33c 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -121,23 +121,28 @@
ceres::examples::VectorOfConstraints target_constraints;
for (auto detection = timestamped_target_detections.begin() + 1;
detection < timestamped_target_detections.end(); detection++) {
- auto last_detection = detection - 1;
+ for (int past = 1;
+ past <=
+ std::min<int>(4, detection - timestamped_target_detections.begin());
+ ++past) {
+ auto last_detection = detection - past;
- // Skip two consecutive detections of the same target, because the solver
- // doesn't allow this
- if (detection->id == last_detection->id) {
- continue;
+ // Skip two consecutive detections of the same target, because the solver
+ // doesn't allow this
+ if (detection->id == last_detection->id) {
+ continue;
+ }
+
+ // Don't take into account constraints too far apart in time, because the
+ // recording device could have moved too much
+ if ((detection->time - last_detection->time) > max_dt) {
+ continue;
+ }
+
+ auto confidence = ComputeConfidence(*last_detection, *detection);
+ target_constraints.emplace_back(
+ ComputeTargetConstraint(*last_detection, *detection, confidence));
}
-
- // Don't take into account constraints too far apart in time, because the
- // recording device could have moved too much
- if ((detection->time - last_detection->time) > max_dt) {
- continue;
- }
-
- auto confidence = ComputeConfidence(*last_detection, *detection);
- target_constraints.emplace_back(
- ComputeTargetConstraint(*last_detection, *detection, confidence));
}
return target_constraints;