Moving calibration file naming to a centralized call, for consistency
Moving to format of:
calibration_NODE_NAME-TEAM#-CAMERA#_cam-24-XX_TIMESTAMP.json
Change-Id: Iece87428a9428cbdd70496f7335f5ec977357200
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index d96822b..46b7cea 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -373,6 +373,7 @@
"//frc971/vision:calibration_fbs",
"//third_party:opencv",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings:str_format",
],
)
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index c48c12d..59a45ac 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -249,18 +249,12 @@
std::stringstream time_ss;
time_ss << realtime_now;
- std::string camera_number_optional = "";
std::optional<uint16_t> camera_number =
frc971::vision::CameraNumberFromChannel(camera_channel_);
- if (camera_number != std::nullopt) {
- camera_number_optional = "-" + std::to_string(camera_number.value());
- }
- const std::string calibration_filename =
- calibration_folder_ +
- absl::StrFormat("/calibration_%s-%d-%d%s_cam-%s_%s.json",
- cpu_type_.value(), team_number.value(),
- cpu_number_.value(), camera_number_optional, camera_id_,
- time_ss.str());
+ CHECK(camera_number.has_value());
+ std::string calibration_filename =
+ CalibrationFilename(calibration_folder_, hostname_, team_number.value(),
+ camera_number.value(), camera_id_, time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(camera_calibration,
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index bfd6209..45aa199 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -1,5 +1,6 @@
#include "frc971/vision/vision_util_lib.h"
+#include "absl/strings/str_format.h"
#include "glog/logging.h"
namespace frc971::vision {
@@ -58,4 +59,19 @@
return camera_number;
}
+std::string CalibrationFilename(std::string calibration_folder,
+ std::string node_name, int team_number,
+ int camera_number, std::string camera_id,
+ std::string timestamp) {
+ // Get rid of any fractional seconds-- we shouldn't need those and it makes
+ // the string unnecessarily longer
+ timestamp = timestamp.substr(0, timestamp.find("."));
+ std::string calibration_filename =
+ calibration_folder +
+ absl::StrFormat("/calibration_%s-%d-%d_cam-%s_%s.json", node_name.c_str(),
+ team_number, camera_number, camera_id.c_str(),
+ timestamp.c_str());
+ return calibration_filename;
+}
+
} // namespace frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
index 8ce651c..a26af3b 100644
--- a/frc971/vision/vision_util_lib.h
+++ b/frc971/vision/vision_util_lib.h
@@ -25,6 +25,12 @@
// not have a number
std::optional<uint16_t> CameraNumberFromChannel(std::string camera_channel);
+// Return a calibration filename to save to based on the given data
+std::string CalibrationFilename(std::string calibration_folder,
+ std::string node_name, int team_number,
+ int camera_number, std::string camera_id,
+ std::string timestamp);
+
} // namespace frc971::vision
#endif // FRC971_VISION_VISION_UTIL_LIB_H_
diff --git a/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 8a4bbaa..35f01db 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -11,6 +11,7 @@
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_util_lib.h"
#include "frc971/vision/visualize_robot.h"
// clang-format off
// OpenCV eigen files must be included after Eigen includes
@@ -47,7 +48,6 @@
DEFINE_uint64(
wait_key, 1,
"Time in ms to wait between images (0 to wait indefinitely until click)");
-
DEFINE_bool(robot, false,
"If true we're calibrating extrinsics for the robot, use the "
"correct node path for the robot.");
@@ -364,7 +364,7 @@
void HandleTargetMap(const TargetMap &map,
aos::distributed_clock::time_point distributed_eof,
std::string node_name) {
- VLOG(1) << "Got april tag map call from node " << node_name;
+ VLOG(1) << "Got april tag map call from camera " << node_name;
// Create empty RGB image in this case
cv::Mat rgb_image;
std::vector<TargetMapper::TargetPose> target_poses;
@@ -478,6 +478,7 @@
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());
@@ -507,20 +508,22 @@
detection_event_loops.back()->MakeWatcher(
camera_node.camera_name(),
- [&reader, &detection_event_loops, camera_node](const TargetMap &map) {
+ [&reader, &detection_event_loops, camera_node,
+ camera_count](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
reader.event_loop_factory()
->GetNodeEventLoopFactory(
- detection_event_loops.back().get()->node())
+ detection_event_loops.at(camera_count).get()->node())
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(
map.monotonic_timestamp_ns())));
HandleTargetMap(map, pi_distributed_time, camera_node.camera_name());
});
- LOG(INFO) << "Created watcher for using the detection event loop for "
- << camera_node.camera_name() << " and size "
- << detection_event_loops.size();
+ 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();
@@ -564,6 +567,8 @@
for (uint i = 0; i < node_list.size() - 1; i++) {
H_camera1_camera2_list.clear();
// Go through the list, and find successive pairs of cameras
+ // We'll be calculating and writing the second of the pair (the i+1'th
+ // camera)
for (auto [pose1, pose2] : detection_list) {
if ((pose1.pi_name == node_list.at(i).camera_name()) &&
(pose2.pi_name == node_list.at(i + 1).camera_name())) {
@@ -576,7 +581,8 @@
}
// Now, get the camera2->boardA map (notice it's boardA, same as
- // camera1, so we can compute the difference based both on boardA)
+ // camera1, so we can compute the difference between the cameras with
+ // both referenced to boardA)
Eigen::Affine3d H_camera2_boardA = pose2.H_camera_target;
// If pose2 isn't referenced to boardA (base_target_id), correct
// that
@@ -682,17 +688,12 @@
std::stringstream time_ss;
time_ss << realtime_now;
- // TODO: This breaks because we're naming orin1 and imu as nodes
- // Assumes camera_list name is of form "/orin#/cameraX" to create
- // calibration filename
+ CameraNode &camera_node = node_list[i + 1];
const std::string calibration_filename =
- FLAGS_output_folder +
- absl::StrFormat(
- "/calibration_orin-%d-%s-%d_cam-%s_%s.json", FLAGS_team_number,
- node_list.at(i + 1).camera_name().substr(5, 1),
- calibration_list[i + 1]->camera_number(),
- calibration_list[i + 1]->camera_id()->data(), time_ss.str());
-
+ frc971::vision::CalibrationFilename(
+ FLAGS_output_folder, camera_node.node_name, FLAGS_team_number,
+ camera_node.camera_number, cal_copy.message().camera_id()->data(),
+ time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(merged_calibration,
{.multi_line = true});
diff --git a/y2024/vision/modify_extrinsics.cc b/y2024/vision/modify_extrinsics.cc
index 9122191..d021744 100644
--- a/y2024/vision/modify_extrinsics.cc
+++ b/y2024/vision/modify_extrinsics.cc
@@ -14,6 +14,7 @@
#include "aos/time/time.h"
#include "aos/util/file.h"
#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/vision_util_lib.h"
// This is a helper program to build and rename calibration files
// You can:
@@ -160,11 +161,9 @@
(FLAGS_calibration_folder == ""
? std::filesystem::path(orig_calib_filename).parent_path().string()
: FLAGS_calibration_folder);
- const std::string new_calib_filename =
- dirname + "/" +
- absl::StrFormat("calibration_%s-%d-%d_cam-%s_%s.json", node_name.c_str(),
- team_number, camera_number, camera_id.c_str(),
- time_ss.str());
+ const std::string new_calib_filename = frc971::vision::CalibrationFilename(
+ dirname, node_name.c_str(), team_number, camera_number, camera_id.c_str(),
+ time_ss.str());
VLOG(1) << "From: " << orig_calib_filename << " -> "
<< aos::FlatbufferToJson(base_calibration, {.multi_line = true});