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;
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index be70754..3beaae0 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -11,13 +11,13 @@
"calibration": {% include 'y2024/constants/calib_files/calibration_imu-7971-0_cam-24-01_2024-03-02_19-44-12.098903651.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json' %}
}
],
"robot": {
diff --git a/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
index d60cc2a..804e854 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-02_20-09-18.016860291.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-0_cam-24-04_2024-03-03_00-43-52.104451759.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- -0.845268,
- 0.031126,
- 0.533435,
- 0.494822,
- -0.525295,
- 0.134521,
- -0.84022,
- -1.212857,
- -0.097911,
- -0.990422,
- -0.097356,
- -0.319412,
+ -0.999409,
+ -0.033871,
+ -0.005938,
+ -0.054149,
+ 0.00624,
+ -0.008812,
+ -0.999942,
+ -0.455382,
+ 0.033816,
+ -0.999387,
+ 0.009018,
+ -0.038435,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
0.000099,
-0.005468
],
- "calibration_timestamp": 1709438958016860291,
+ "calibration_timestamp": 1709455432104451759,
"camera_id": "24-04",
"camera_number": 0
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json
rename to y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
index ed7b2f3..5e9095f 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-02_20-09-18.022901293.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-1-1_cam-24-02_2024-03-03_00-43-52.104348086.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- -0.07336,
- 0.085699,
- 0.993617,
- 0.608349,
- -0.989433,
- 0.118683,
- -0.083288,
- -0.939084,
- -0.125063,
- -0.989227,
- 0.076087,
- -0.35071,
+ -0.465205,
+ -0.009078,
+ 0.885157,
+ -0.040515,
+ -0.885133,
+ -0.007831,
+ -0.465273,
+ -0.328485,
+ 0.011156,
+ -0.999928,
+ -0.004392,
+ -0.036284,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
0.000015,
-0.005636
],
- "calibration_timestamp": 1709438958022901293,
+ "calibration_timestamp": 1709455432104348086,
"camera_id": "24-02",
"camera_number": 1
}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json b/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
similarity index 62%
rename from y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json
rename to y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
index c0847a8..874d282 100755
--- a/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-02_20-09-18.016217514.json
+++ b/y2024/constants/calib_files/calibration_orin-7971-c-1_cam-24-03_2024-03-03_00-43-52.104098137.json
@@ -14,18 +14,18 @@
],
"fixed_extrinsics": {
"data": [
- 0.646787,
- 0.04307,
- 0.761453,
- 0.280984,
- -0.762291,
- 0.067995,
- 0.643653,
- -0.238549,
- -0.024053,
- -0.996756,
- 0.07681,
- -0.200534,
+ 0.530963,
+ -0.006085,
+ 0.847373,
+ -0.047041,
+ -0.847373,
+ -0.011096,
+ 0.530883,
+ -0.127507,
+ 0.006172,
+ -0.99992,
+ -0.011048,
+ -0.04483,
0.0,
0.0,
0.0,
@@ -39,7 +39,7 @@
-0.000061,
-0.00879
],
- "calibration_timestamp": 1709438958016217514,
+ "calibration_timestamp": 1709455432104098137,
"camera_id": "24-03",
"camera_number": 1
}
\ No newline at end of file
diff --git a/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 35f01db..dee6646 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -478,7 +478,6 @@
std::vector<frc971::vision::ImageCallback *> image_callbacks;
std::vector<Eigen::Affine3d> default_extrinsics;
- uint camera_count = 0;
for (const CameraNode &camera_node : node_list) {
const aos::Node *pi = aos::configuration::GetNode(
reader.configuration(), camera_node.node_name.c_str());
@@ -486,8 +485,9 @@
detection_event_loops.emplace_back(
reader.event_loop_factory()->MakeEventLoop(
(camera_node.camera_name() + "_detection").c_str(), pi));
+ aos::EventLoop *const event_loop = detection_event_loops.back().get();
frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
- detection_event_loops.back().get());
+ event_loop);
// Get the calibration for this orin/camera pair
const calibration::CameraCalibration *calibration =
y2024::vision::FindCameraCalibration(constants_fetcher.constants(),
@@ -506,14 +506,12 @@
VLOG(1) << "Got extrinsics for " << camera_node.camera_name() << " as\n"
<< default_extrinsics.back().matrix();
- detection_event_loops.back()->MakeWatcher(
+ event_loop->MakeWatcher(
camera_node.camera_name(),
- [&reader, &detection_event_loops, camera_node,
- camera_count](const TargetMap &map) {
+ [&reader, event_loop, camera_node](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
reader.event_loop_factory()
- ->GetNodeEventLoopFactory(
- detection_event_loops.at(camera_count).get()->node())
+ ->GetNodeEventLoopFactory(event_loop->node())
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(
map.monotonic_timestamp_ns())));
@@ -523,7 +521,6 @@
VLOG(1) << "Created watcher for using the detection event loop for "
<< camera_node.camera_name() << " and size "
<< detection_event_loops.size();
- camera_count++;
}
reader.event_loop_factory()->Run();
diff --git a/y2024/vision/target_mapping.cc b/y2024/vision/target_mapping.cc
index 7548445..d2fb26e 100644
--- a/y2024/vision/target_mapping.cc
+++ b/y2024/vision/target_mapping.cc
@@ -389,6 +389,7 @@
target_constraints.erase(
std::remove_if(target_constraints.begin(), target_constraints.end(),
[](const auto &constraint) {
+ // TODO(james): This no longer makes sense.
constexpr TargetMapper::TargetId kMaxRedId = 4;
TargetMapper::TargetId min_id =
std::min(constraint.id_begin, constraint.id_end);
diff --git a/y2024/y2024_orin1.json b/y2024/y2024_orin1.json
index f09be40..97c6b64 100644
--- a/y2024/y2024_orin1.json
+++ b/y2024/y2024_orin1.json
@@ -141,6 +141,7 @@
"name": "/orin1/camera0",
"type": "foxglove.CompressedImage",
"source_node": "orin1",
+ "logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 622384
@@ -149,6 +150,7 @@
"name": "/orin1/camera1",
"type": "foxglove.CompressedImage",
"source_node": "orin1",
+ "logger": "NOT_LOGGED",
"channel_storage_duration": 1000000000,
"frequency": 65,
"max_size": 622384