Merge "Move extend to canfd"
diff --git a/frc971/vision/calibration.fbs b/frc971/vision/calibration.fbs
index 64a5d3c..99b6228 100644
--- a/frc971/vision/calibration.fbs
+++ b/frc971/vision/calibration.fbs
@@ -8,7 +8,7 @@
// Calibration information for a given camera on a given robot.
table CameraCalibration {
- // The name of the camera node which this calibration data applies to.
+ // The name of the compute node which this calibration data applies to.
node_name:string (id: 0);
// The team number of the robot which this calibration data applies to.
team_number:int (id: 1);
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 38b63d7..dbdc3d9 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -8,16 +8,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-06_1970-01-05_17-40-29.245444672.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-07_2024-02-24_19-52-27.338466592.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json' %}
},
{
- "calibration": {% include 'y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json' %}
+ "calibration": {% include 'y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json' %}
}
],
"robot": {
diff --git a/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json b/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json
new file mode 100755
index 0000000..d013e2a
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_imu-971-0_cam-24-07_2024-03-01_11-01-32.895328333.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 971,
+ "intrinsics": [
+ 647.822815,
+ 0.0,
+ 715.37616,
+ 0.0,
+ 647.799316,
+ 494.638641,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 1.0,
+ -0.0,
+ 0.0,
+ 0.111049,
+ 0.0,
+ 0.258819,
+ 0.965926,
+ 0.263806,
+ -0.0,
+ -0.965926,
+ 0.258819,
+ 0.347685,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.2423,
+ 0.057169,
+ 0.000302,
+ 0.000016,
+ -0.005638
+ ],
+ "calibration_timestamp": 1708833147338466592,
+ "camera_id": "24-07",
+ "camera_number": 0,
+ "reprojection_error": 1.362672
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json b/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json
new file mode 100755
index 0000000..fbe79d5
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_imu-971-1_cam-24-08_2024-03-01_11-02-11.982641320.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "imu",
+ "team_number": 971,
+ "intrinsics": [
+ 645.963562,
+ 0.0,
+ 751.21698,
+ 0.0,
+ 645.34906,
+ 605.204102,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ -0.258819,
+ -0.965926,
+ -0.323293,
+ 1.0,
+ 0.0,
+ -0.0,
+ 0.268249,
+ 0.0,
+ -0.965926,
+ 0.258819,
+ 0.471129,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.248733,
+ 0.06221,
+ -0.000901,
+ 0.000128,
+ -0.006595
+ ],
+ "calibration_timestamp": 1708820514420797344,
+ "camera_id": "24-08",
+ "camera_number": 1,
+ "reprojection_error": 1.591953
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json
index b5a848b..14d9bda 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-08_2024-02-24_16-21-54.420797344.json
@@ -1,5 +1,5 @@
{
- "node_name": "orin1",
+ "node_name": "imu",
"team_number": 971,
"intrinsics": [
645.963562,
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json
index 9ef782e..2ac9471 100755
--- a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-05_1970-01-05_17-40-27.793683328.json
@@ -1,5 +1,5 @@
{
- "node_name": "orin1",
+ "node_name": "imu",
"team_number": 971,
"intrinsics": [
648.360168,
diff --git a/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json b/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json
new file mode 100755
index 0000000..317e453
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin1-971-0_cam-24-05_2024-03-01_11-01-05.102438041.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.360168,
+ 0.0,
+ 729.818665,
+ 0.0,
+ 648.210327,
+ 641.988037,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 0.0,
+ 0.0,
+ 1.0,
+ 0.284397,
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.226771,
+ 0.0,
+ -1.0,
+ 0.0,
+ 0.442951,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.255473,
+ 0.068444,
+ 0.000028,
+ -0.000078,
+ -0.008004
+ ],
+ "calibration_timestamp": 409227793683328,
+ "camera_id": "24-05",
+ "camera_number": 0,
+ "reprojection_error": 1.058851
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json b/y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json
new file mode 100755
index 0000000..0eb10db
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin1-971-1_cam-24-06_2024-03-01_11-01-20.409861949.json
@@ -0,0 +1,46 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.644104,
+ 0.0,
+ 755.677979,
+ 0.0,
+ 648.522644,
+ 597.744812,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1.0,
+ 0.0,
+ 0.0,
+ 0.111049,
+ -0.0,
+ -0.258819,
+ -0.965926,
+ -0.263806,
+ 0.0,
+ -0.965926,
+ 0.258819,
+ 0.347685,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ]
+ },
+ "dist_coeffs": [
+ -0.25182,
+ 0.063137,
+ 0.000118,
+ 0.000005,
+ -0.006342
+ ],
+ "calibration_timestamp": 409229245444672,
+ "camera_id": "24-06",
+ "camera_number": 1,
+ "reprojection_error": 1.344104
+}
\ No newline at end of file
diff --git a/y2024/constants/test_data/calibration_cam-3.json b/y2024/constants/test_data/calibration_cam-3.json
index 01d1ac0..16e67ec 100644
--- a/y2024/constants/test_data/calibration_cam-3.json
+++ b/y2024/constants/test_data/calibration_cam-3.json
@@ -1,5 +1,5 @@
{
- "node_name": "orin2",
+ "node_name": "imu",
"camera_number": 0,
"team_number": 7971,
"intrinsics": [
diff --git a/y2024/constants/test_data/calibration_cam-4.json b/y2024/constants/test_data/calibration_cam-4.json
index 3360781..1e5b623 100644
--- a/y2024/constants/test_data/calibration_cam-4.json
+++ b/y2024/constants/test_data/calibration_cam-4.json
@@ -1,5 +1,5 @@
{
- "node_name": "orin2",
+ "node_name": "imu",
"camera_number": 1,
"team_number": 7971,
"intrinsics": [
diff --git a/y2024/localizer/corrections_plotter.ts b/y2024/localizer/corrections_plotter.ts
index dbfbda6..fcd56eb 100644
--- a/y2024/localizer/corrections_plotter.ts
+++ b/y2024/localizer/corrections_plotter.ts
@@ -42,7 +42,7 @@
const targets = [];
const targetLabels = [];
- for (const orin of ['orin1', 'orin2']) {
+ for (const orin of ['orin1', 'imu']) {
for (const camera of ['camera0', 'camera1']) {
targetLabels.push(orin + ' ' + camera);
targets.push(aosPlotter.addRawMessageSource(
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index 34c0bc7..441360c 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -34,8 +34,8 @@
namespace y2024::localizer {
namespace {
constexpr std::array<std::string_view, Localizer::kNumCameras>
- kDetectionChannels{"/orin1/camera0", "/orin1/camera1", "/orin2/camera0",
- "/orin2/camera1"};
+ kDetectionChannels{"/orin1/camera0", "/orin1/camera1", "/imu/camera0",
+ "/imu/camera1"};
size_t CameraIndexForName(std::string_view name) {
for (size_t index = 0; index < kDetectionChannels.size(); ++index) {
diff --git a/y2024/localizer/localizer_replay.cc b/y2024/localizer/localizer_replay.cc
index 763ad36..c2c4c16 100644
--- a/y2024/localizer/localizer_replay.cc
+++ b/y2024/localizer/localizer_replay.cc
@@ -33,7 +33,7 @@
aos::logger::LogReader reader(logfiles, &config.message());
reader.RemapLoggedChannel("/localizer", "y2024.localizer.Status");
- for (const auto orin : {"orin1", "orin2"}) {
+ for (const auto orin : {"orin1", "imu"}) {
for (const auto camera : {"camera0", "camera1"}) {
reader.RemapLoggedChannel(absl::StrCat("/", orin, "/", camera),
"y2024.localizer.Visualization");
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
index 4b515cf..aab6e64 100644
--- a/y2024/vision/BUILD
+++ b/y2024/vision/BUILD
@@ -115,3 +115,22 @@
"@org_tuxfamily_eigen//:eigen",
],
)
+
+cc_binary(
+ name = "modify_extrinsics",
+ srcs = [
+ "modify_extrinsics.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024:__subpackages__"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:vision_util_lib",
+ "//y2024/constants:constants_fbs",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/y2024/vision/apriltag_detector.cc b/y2024/vision/apriltag_detector.cc
index 96ff869..5edcf36 100644
--- a/y2024/vision/apriltag_detector.cc
+++ b/y2024/vision/apriltag_detector.cc
@@ -20,10 +20,12 @@
const frc971::constants::ConstantsFetcher<y2024::Constants> calibration_data(
&event_loop);
+ CHECK(FLAGS_channel.length() == 8);
+ int camera_id = std::stoi(FLAGS_channel.substr(7, 1));
const frc971::vision::calibration::CameraCalibration *calibration =
y2024::vision::FindCameraCalibration(
calibration_data.constants(),
- event_loop.node()->name()->string_view());
+ event_loop.node()->name()->string_view(), camera_id);
frc971::apriltag::ApriltagDetector detector(&event_loop, FLAGS_channel,
calibration);
diff --git a/y2024/vision/calibrate_multi_cameras.cc b/y2024/vision/calibrate_multi_cameras.cc
index 9148217..f743e00 100644
--- a/y2024/vision/calibrate_multi_cameras.cc
+++ b/y2024/vision/calibrate_multi_cameras.cc
@@ -31,7 +31,7 @@
"If true, show visualization from field level, rather than above");
DEFINE_string(config, "",
"If set, override the log's config file with this one.");
-DEFINE_string(constants_path, "y2023/constants/constants.json",
+DEFINE_string(constants_path, "y2024/constants/constants.json",
"Path to the constant file");
DEFINE_double(max_pose_error, 5e-5,
"Throw out target poses with a higher pose error than this");
@@ -64,11 +64,11 @@
// and then map each subsequent camera based on the data collected and
// the extrinsic poses computed here.
-// TODO<Jim>: Not currently using estimate from pi1->pi4-- should do full
-// estimation, and probably also include camera->imu extrinsics from all
-// cameras, not just pi1
+// TODO<Jim>: Not currently using estimate from first camera to last camera--
+// should do full estimation, and probably also include camera->imu extrinsics
+// from all cameras, not just first camera
-namespace y2023::vision {
+namespace y2024::vision {
using frc971::vision::DataAdapter;
using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
@@ -425,15 +425,8 @@
aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
config.has_value() ? &config->message() : nullptr);
- constexpr size_t kNumPis = 4;
- for (size_t i = 1; i <= kNumPis; i++) {
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
- "y2023.Constants");
- }
-
- reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
- reader.RemapLoggedChannel("/logger/constants", "y2023.Constants");
- reader.RemapLoggedChannel("/roborio/constants", "y2023.Constants");
+ reader.RemapLoggedChannel("/imu/constants", "y2024.Constants");
+ reader.RemapLoggedChannel("/orin1/constants", "y2024.Constants");
reader.Register();
y2024::SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
@@ -441,10 +434,9 @@
VLOG(1) << "Using target type " << FLAGS_target_type;
std::vector<std::string> node_list;
- node_list.push_back("pi1");
- node_list.push_back("pi2");
- node_list.push_back("pi3");
- node_list.push_back("pi4");
+ node_list.push_back("orin1");
+ node_list.push_back("imu");
+ std::vector<std::string> camera_list;
std::vector<const calibration::CameraCalibration *> calibration_list;
std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
@@ -461,65 +453,76 @@
reader.event_loop_factory()->MakeEventLoop(
(node + "_detection").c_str(), pi));
- frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
- detection_event_loops.back().get());
+ for (const std::string camera : {"/camera0", "/camera1"}) {
+ std::string camera_name = "/" + node + camera;
+ camera_list.push_back(camera_name);
+ frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
+ detection_event_loops.back().get());
- const calibration::CameraCalibration *calibration =
- y2024::vision::FindCameraCalibration(constants_fetcher.constants(),
- node);
- calibration_list.push_back(calibration);
+ // Get the calibration for this orin/camera pair
+ int camera_id = std::stoi(camera.substr(7, 1));
+ const calibration::CameraCalibration *calibration =
+ y2024::vision::FindCameraCalibration(constants_fetcher.constants(),
+ node, camera_id);
+ calibration_list.push_back(calibration);
- frc971::vision::TargetType target_type =
- frc971::vision::TargetTypeFromString(FLAGS_target_type);
- frc971::vision::CharucoExtractor *charuco_ext =
- new frc971::vision::CharucoExtractor(calibration, target_type);
- charuco_extractors.emplace_back(charuco_ext);
+ // Extract the extrinsics from the calibration, and save as "defaults"
+ cv::Mat extrinsics_cv =
+ frc971::vision::CameraExtrinsics(calibration).value();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto ext_H_robot_pi = Eigen::Affine3d(extrinsics_matrix);
+ default_extrinsics.emplace_back(ext_H_robot_pi);
- cv::Mat extrinsics_cv =
- frc971::vision::CameraExtrinsics(calibration).value();
- Eigen::Matrix4d extrinsics_matrix;
- cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
- const auto ext_H_robot_pi = Eigen::Affine3d(extrinsics_matrix);
- default_extrinsics.emplace_back(ext_H_robot_pi);
+ VLOG(1) << "Got extrinsics for " << node << ", " << camera << " as\n"
+ << default_extrinsics.back().matrix();
- VLOG(1) << "Got extrinsics for " << node << " as\n"
- << default_extrinsics.back().matrix();
+ // If we've got full logs, we need to set up a charuco/april tag extractor
+ // nd a call back to handle the data
+ if (FLAGS_use_full_logs) {
+ frc971::vision::TargetType target_type =
+ frc971::vision::TargetTypeFromString(FLAGS_target_type);
+ frc971::vision::CharucoExtractor *charuco_ext =
+ new frc971::vision::CharucoExtractor(calibration, target_type);
+ charuco_extractors.emplace_back(charuco_ext);
- if (FLAGS_use_full_logs) {
- LOG(INFO) << "Set up image callback for node " << node_list[i];
- frc971::vision::ImageCallback *image_callback =
- new frc971::vision::ImageCallback(
- detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
- [&reader, &charuco_extractors, &detection_event_loops, &node_list,
- i](cv::Mat rgb_image,
- const aos::monotonic_clock::time_point eof) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader.event_loop_factory()
- ->GetNodeEventLoopFactory(
- detection_event_loops[i].get()->node())
- ->ToDistributedClock(eof);
- HandleImage(detection_event_loops[i].get(), rgb_image, eof,
- pi_distributed_time, *charuco_extractors[i],
- node_list[i]);
- });
+ LOG(INFO) << "Set up image callback for node " << node << ", camera "
+ << camera;
+ frc971::vision::ImageCallback *image_callback =
+ new frc971::vision::ImageCallback(
+ detection_event_loops[i].get(), camera_name,
+ [&reader, &charuco_extractors, &detection_event_loops,
+ &node_list, i](cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(
+ detection_event_loops[i].get()->node())
+ ->ToDistributedClock(eof);
+ HandleImage(detection_event_loops[i].get(), rgb_image, eof,
+ pi_distributed_time, *charuco_extractors[i],
+ node_list[i]);
+ });
- image_callbacks.emplace_back(image_callback);
- } else {
- detection_event_loops[i]->MakeWatcher(
- "/camera", [&reader, &detection_event_loops, &node_list,
- i](const TargetMap &map) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader.event_loop_factory()
- ->GetNodeEventLoopFactory(detection_event_loops[i]->node())
- ->ToDistributedClock(aos::monotonic_clock::time_point(
- aos::monotonic_clock::duration(
- map.monotonic_timestamp_ns())));
+ image_callbacks.emplace_back(image_callback);
+ } else {
+ detection_event_loops[i]->MakeWatcher(
+ camera, [&reader, &detection_event_loops, camera_name,
+ i](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(
+ detection_event_loops[i]->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(
+ map.monotonic_timestamp_ns())));
- HandleTargetMap(map, pi_distributed_time, node_list[i]);
- });
- LOG(INFO) << "Created watcher for using the detection event loop for "
- << node_list[i] << " with i = " << i << " and size "
- << detection_event_loops.size();
+ HandleTargetMap(map, pi_distributed_time, camera_name);
+ });
+ LOG(INFO) << "Created watcher for using the detection event loop for "
+ << node << " with i = " << i << " and size "
+ << detection_event_loops.size() << " and camera " << camera;
+ }
}
}
@@ -531,17 +534,17 @@
<< "Must have at least one view of both boards";
int base_target_id = two_board_extrinsics_list[0].board_id;
VLOG(1) << "Base id for two_board_extrinsics_list is " << base_target_id;
- for (auto node : node_list) {
+ for (auto camera : camera_list) {
std::vector<TimestampedPiDetection> pose_list;
for (auto ext : two_board_extrinsics_list) {
CHECK_EQ(base_target_id, ext.board_id)
<< " All boards should have same reference id";
- if (ext.pi_name == node) {
+ if (ext.pi_name == camera) {
pose_list.push_back(ext);
}
}
Eigen::Affine3d avg_pose_from_pi = ComputeAveragePose(pose_list);
- VLOG(1) << "Estimate from " << node << " with " << pose_list.size()
+ VLOG(1) << "Estimate from " << camera << " with " << pose_list.size()
<< " observations is:\n"
<< avg_pose_from_pi.matrix();
}
@@ -558,14 +561,14 @@
std::vector<Eigen::Affine3d> updated_extrinsics;
// Use the first node's extrinsics as our base, and fix from there
updated_extrinsics.push_back(default_extrinsics[0]);
- LOG(INFO) << "Default extrinsic for node " << node_list[0] << " is "
+ LOG(INFO) << "Default extrinsic for camera " << camera_list[0] << " is "
<< default_extrinsics[0].matrix();
- for (uint i = 0; i < node_list.size() - 1; i++) {
+ for (uint i = 0; i < camera_list.size() - 1; i++) {
H_camera1_camera2_list.clear();
// Go through the list, and find successive pairs of cameras
for (auto [pose1, pose2] : detection_list) {
- if ((pose1.pi_name == node_list[i]) &&
- (pose2.pi_name == node_list[i + 1])) {
+ if ((pose1.pi_name == camera_list[i]) &&
+ (pose2.pi_name == camera_list[i + 1])) {
Eigen::Affine3d H_camera1_boardA = pose1.H_camera_target;
// If pose1 isn't referenced to base_target_id, correct that
if (pose1.board_id != base_target_id) {
@@ -613,11 +616,11 @@
// TODO<Jim>: If we don't get any matches, we could just use default
// extrinsics
CHECK(H_camera1_camera2_list.size() > 0)
- << "Failed with zero poses for node " << node_list[i];
+ << "Failed with zero poses for node " << camera_list[i];
if (H_camera1_camera2_list.size() > 0) {
Eigen::Affine3d H_camera1_camera2_avg =
ComputeAveragePose(H_camera1_camera2_list);
- LOG(INFO) << "From " << node_list[i] << " to " << node_list[i + 1]
+ LOG(INFO) << "From " << camera_list[i] << " to " << camera_list[i + 1]
<< " found " << H_camera1_camera2_list.size()
<< " observations, and the average pose is:\n"
<< H_camera1_camera2_avg.matrix();
@@ -640,9 +643,10 @@
Eigen::Affine3d next_extrinsic =
updated_extrinsics.back() * H_camera1_camera2_avg;
updated_extrinsics.push_back(next_extrinsic);
- LOG(INFO) << "Default Extrinsic for " << node_list[i + 1] << " is \n"
+ LOG(INFO) << "Default Extrinsic for " << camera_list[i + 1] << " is \n"
<< default_extrinsics[i + 1].matrix();
- LOG(INFO) << "--> Updated Extrinsic for " << node_list[i + 1] << " is \n"
+ LOG(INFO) << "--> Updated Extrinsic for " << camera_list[i + 1]
+ << " is \n"
<< next_extrinsic.matrix();
// Wirte out this extrinsic to a file
@@ -677,11 +681,14 @@
std::stringstream time_ss;
time_ss << realtime_now;
- // Assumes node_list name is of form "pi#" to create camera id
+ // 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
const std::string calibration_filename =
FLAGS_output_folder +
- absl::StrFormat("/calibration_pi-%d-%s_cam-%s_%s.json",
- FLAGS_team_number, node_list[i + 1].substr(2, 3),
+ absl::StrFormat("/calibration_orin-%d-%s-%d_cam-%s_%s.json",
+ FLAGS_team_number, camera_list[i + 1].substr(5, 1),
+ calibration_list[i + 1]->camera_number(),
calibration_list[i + 1]->camera_id()->data(),
time_ss.str());
@@ -701,9 +708,9 @@
delete image_callbacks[i];
}
}
-} // namespace y2023::vision
+} // namespace y2024::vision
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- y2023::vision::ExtrinsicsMain(argc, argv);
+ y2024::vision::ExtrinsicsMain(argc, argv);
}
diff --git a/y2024/vision/modify_extrinsics.cc b/y2024/vision/modify_extrinsics.cc
new file mode 100644
index 0000000..9122191
--- /dev/null
+++ b/y2024/vision/modify_extrinsics.cc
@@ -0,0 +1,189 @@
+#include <cmath>
+#include <filesystem>
+#include <regex>
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "absl/strings/str_format.h"
+
+#include "aos/configuration.h"
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/vision/calibration_generated.h"
+
+// This is a helper program to build and rename calibration files
+// You can:
+// (1) pass it in a new set of orin #, team #, camera #, to rename the file
+// (2) Pass in extrinsics to set the extrinsics
+// By default, writes to /tmp, but will write to calib_files folder if
+// full path is given and calibration_folder is blank
+
+DEFINE_string(orig_calib_file, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+DEFINE_string(calibration_folder, "/tmp", "Folder to place calibration files.");
+DEFINE_string(node_name, "",
+ "Node name to use, e.g. orin1, imu; unchanged if blank");
+DEFINE_int32(team_number, -1, "Team number to use; unchanged if -1");
+DEFINE_int32(camera_number, -1, "Camera number to use; unchanged if -1");
+
+DEFINE_bool(set_extrinsics, true, "Set to false to ignore extrinsic data");
+DEFINE_bool(use_inches, true,
+ "Whether to use inches as units (meters if false)");
+DEFINE_bool(use_degrees, true,
+ "Whether to use degrees as units (radians if false)");
+DEFINE_double(camera_x, 0.0, "x location of camera");
+DEFINE_double(camera_y, 0.0, "y location of camera");
+DEFINE_double(camera_z, 0.0, "z location of camera");
+// Don't currently allow for roll of cameras
+DEFINE_double(camera_yaw, 0.0, "yaw of camera about robot z axis");
+DEFINE_double(camera_pitch, 0.0, "pitch of camera relative to robot y axis");
+// TODO: This could be done by setting the pixel size and using the intrinsics
+DEFINE_double(focal_length, 0.002, "Focal length in meters");
+
+namespace frc971::vision {
+namespace {
+
+// TODO: Put this in vision_util_lib? Except, it depends on Eigen
+std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H) {
+ std::vector<float> data;
+ for (int row = 0; row < 4; ++row) {
+ for (int col = 0; col < 4; ++col) {
+ data.push_back(H(row, col));
+ }
+ }
+ return data;
+}
+
+// Merge the original calibration file with all its changes
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> BuildCalibration(
+ const frc971::vision::calibration::CameraCalibration *calibration,
+ std::string node_name, uint16_t camera_number, uint16_t team_number) {
+ aos::FlatbufferDetachedBuffer<frc971::vision::calibration::CameraCalibration>
+ cal_copy = aos::RecursiveCopyFlatBuffer(calibration);
+
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::String> node_name_offset =
+ fbb.CreateString(absl::StrFormat("%s", node_name.c_str()));
+
+ // If we're told to set the extrinsics, clear old and add in new
+ flatbuffers::Offset<calibration::TransformationMatrix>
+ fixed_extrinsics_offset;
+ if (FLAGS_set_extrinsics) {
+ cal_copy.mutable_message()->clear_fixed_extrinsics();
+ Eigen::Affine3d extrinsic_matrix;
+ // Convert to metric
+ double translation_scale = (FLAGS_use_inches ? 0.0254 : 1.0);
+ Eigen::Translation3d translation(FLAGS_camera_x * translation_scale,
+ FLAGS_camera_y * translation_scale,
+ FLAGS_camera_z * translation_scale);
+
+ // convert to radians
+ double angle_scale = (FLAGS_use_degrees ? M_PI / 180.0 : 1.0);
+ // The rotation that takes robot coordinates (x forward, z up) to camera
+ // coordiantes (z forward, x right)
+ Eigen::Quaterniond R_robo_cam =
+ Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitX());
+ Eigen::AngleAxisd pitchAngle(FLAGS_camera_pitch * angle_scale,
+ Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yawAngle(FLAGS_camera_yaw * angle_scale,
+ Eigen::Vector3d::UnitZ());
+
+ Eigen::Quaterniond rotation = yawAngle * pitchAngle * R_robo_cam;
+ Eigen::Vector3d focal_length_offset =
+ (rotation * Eigen::Translation3d(0.0, 0.0, FLAGS_focal_length))
+ .translation();
+ translation = translation * Eigen::Translation3d(focal_length_offset);
+
+ extrinsic_matrix = translation * rotation;
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(
+ frc971::vision::MatrixToVector(extrinsic_matrix.matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ fixed_extrinsics_offset = matrix_builder.Finish();
+ }
+
+ calibration::CameraCalibration::Builder camera_calibration_builder(fbb);
+ camera_calibration_builder.add_node_name(node_name_offset);
+ camera_calibration_builder.add_team_number(team_number);
+ if (FLAGS_set_extrinsics) {
+ camera_calibration_builder.add_fixed_extrinsics(fixed_extrinsics_offset);
+ }
+ camera_calibration_builder.add_camera_number(camera_number);
+ fbb.Finish(camera_calibration_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> updated_cal =
+ fbb.Release();
+
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ merged_calibration =
+ aos::MergeFlatBuffers(&cal_copy.message(), &updated_cal.message());
+ return merged_calibration;
+}
+
+void Main(std::string orig_calib_filename) {
+ LOG(INFO) << "Reading from file: " << orig_calib_filename;
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ base_calibration =
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ orig_calib_filename);
+
+ // Populate the new variables from command-line or from base_calibration
+ std::string node_name =
+ (FLAGS_node_name == "" ? base_calibration.message().node_name()->str()
+ : FLAGS_node_name);
+ int team_number =
+ (FLAGS_team_number == -1 ? base_calibration.message().team_number()
+ : FLAGS_team_number);
+ int camera_number =
+ (FLAGS_camera_number == -1 ? base_calibration.message().camera_number()
+ : FLAGS_camera_number);
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ new_calibration = BuildCalibration(&base_calibration.message(), node_name,
+ camera_number, team_number);
+
+ // Set a new timestamp on the file, but leave calibration_timestamp unchanged
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+ // Use the camera_id that is set in the json file (not the filename)
+ std::string camera_id = base_calibration.message().camera_id()->str();
+
+ const std::string dirname =
+ (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());
+
+ VLOG(1) << "From: " << orig_calib_filename << " -> "
+ << aos::FlatbufferToJson(base_calibration, {.multi_line = true});
+
+ VLOG(1) << "Writing: " << new_calib_filename << " -> "
+ << aos::FlatbufferToJson(new_calibration, {.multi_line = true});
+
+ LOG(INFO) << "Writing to file: " << new_calib_filename;
+ aos::util::WriteStringToFileOrDie(
+ new_calib_filename,
+ aos::FlatbufferToJson(new_calibration, {.multi_line = true}));
+}
+
+} // namespace
+} // namespace frc971::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ CHECK(argc == 2) << "Must supply a starting calibration filename";
+ std::string filename = argv[1];
+ frc971::vision::Main(filename);
+}
diff --git a/y2024/vision/viewer.cc b/y2024/vision/viewer.cc
index 6f83ec2..d5ada14 100644
--- a/y2024/vision/viewer.cc
+++ b/y2024/vision/viewer.cc
@@ -85,8 +85,11 @@
frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
&event_loop);
+ CHECK(FLAGS_channel.length() == 8);
+ int camera_id = std::stoi(FLAGS_channel.substr(7, 1));
const auto *calibration_data = FindCameraCalibration(
- constants_fetcher.constants(), event_loop.node()->name()->string_view());
+ constants_fetcher.constants(), event_loop.node()->name()->string_view(),
+ camera_id);
const cv::Mat intrinsics = frc971::vision::CameraIntrinsics(calibration_data);
const cv::Mat dist_coeffs =
frc971::vision::CameraDistCoeffs(calibration_data);
diff --git a/y2024/vision/vision_util.cc b/y2024/vision/vision_util.cc
index 4c196ce..6acd234 100644
--- a/y2024/vision/vision_util.cc
+++ b/y2024/vision/vision_util.cc
@@ -5,17 +5,20 @@
namespace y2024::vision {
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
- const y2024::Constants &calibration_data, std::string_view node_name) {
+ const y2024::Constants &calibration_data, std::string_view node_name,
+ int camera_number) {
CHECK(calibration_data.has_cameras());
for (const y2024::CameraConfiguration *candidate :
*calibration_data.cameras()) {
CHECK(candidate->has_calibration());
- if (candidate->calibration()->node_name()->string_view() != node_name) {
+ if (candidate->calibration()->node_name()->string_view() != node_name ||
+ candidate->calibration()->camera_number() != camera_number) {
continue;
}
return candidate->calibration();
}
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " and camera number " << camera_number;
}
} // namespace y2024::vision
diff --git a/y2024/vision/vision_util.h b/y2024/vision/vision_util.h
index d8fa562..c903760 100644
--- a/y2024/vision/vision_util.h
+++ b/y2024/vision/vision_util.h
@@ -9,7 +9,8 @@
namespace y2024::vision {
const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
- const y2024::Constants &calibration_data, std::string_view node_name);
+ const y2024::Constants &calibration_data, std::string_view node_name,
+ int camera_number);
} // namespace y2024::vision
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 6ab6c03..155edb4 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -171,7 +171,7 @@
"max_size": 200
},
{
- "name": "/orin2/camera0",
+ "name": "/imu/camera0",
"type": "frc971.vision.CameraImage",
"source_node": "imu",
"channel_storage_duration": 1000000000,
@@ -182,7 +182,7 @@
"num_senders": 18
},
{
- "name": "/orin2/camera1",
+ "name": "/imu/camera1",
"type": "frc971.vision.CameraImage",
"source_node": "imu",
"channel_storage_duration": 1000000000,
@@ -193,7 +193,7 @@
"num_senders": 18
},
{
- "name": "/orin2/camera0",
+ "name": "/imu/camera0",
"type": "foxglove.CompressedImage",
"source_node": "imu",
"logger": "NOT_LOGGED",
@@ -202,7 +202,7 @@
"max_size": 622384
},
{
- "name": "/orin2/camera1",
+ "name": "/imu/camera1",
"type": "foxglove.CompressedImage",
"source_node": "imu",
"logger": "NOT_LOGGED",
@@ -211,35 +211,35 @@
"max_size": 622384
},
{
- "name": "/orin2/camera0",
+ "name": "/imu/camera0",
"type": "foxglove.ImageAnnotations",
"source_node": "imu",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin2/camera1",
+ "name": "/imu/camera1",
"type": "foxglove.ImageAnnotations",
"source_node": "imu",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin2/camera0",
+ "name": "/imu/camera0",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin2/camera1",
+ "name": "/imu/camera1",
"type": "y2024.localizer.Visualization",
"source_node": "imu",
"frequency": 65,
"max_size": 50000
},
{
- "name": "/orin2/camera0",
+ "name": "/imu/camera0",
"type": "frc971.vision.TargetMap",
"source_node": "imu",
"frequency": 65,
@@ -247,7 +247,7 @@
"max_size": 1024
},
{
- "name": "/orin2/camera1",
+ "name": "/imu/camera1",
"type": "frc971.vision.TargetMap",
"source_node": "imu",
"frequency": 65,
@@ -550,7 +550,7 @@
"source_node": "imu"
},
"rename": {
- "name": "/orin2/camera"
+ "name": "/imu/camera"
}
}
],