Refactor of apriltag detector to send out tags
Split into app in y2024, and libs in frc971
Added in calibration constants to y2024, including single basic camera
Modified team number parsing to work with orins
Pulled out year-by-year calibration call to a single FindCameraCalibration
call, and rest (extrinsics, intrinsics) are year-agnostic
Moving camera calibration files into constants/calib_files directory, instead
of vision/calib_files
Change-Id: Ic485086aad9665d2b571551a5afb337c9254d690
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 932261d..5d8bf7c 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -357,3 +357,15 @@
"@org_tuxfamily_eigen//:eigen",
],
)
+
+cc_library(
+ name = "vision_util_lib",
+ srcs = ["vision_util_lib.cc"],
+ hdrs = ["vision_util_lib.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:calibration_fbs",
+ "//third_party:opencv",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index d8abd33..2c0e603 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -163,11 +163,13 @@
HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
rvecs_eigen, tvecs_eigen);
}),
+ // TODO: Need to make this work for pi or orin
image_callback_(
image_event_loop_,
- absl::StrCat("/pi",
- std::to_string(aos::network::ParsePiNumber(pi).value()),
- image_channel),
+ absl::StrCat(
+ "/pi",
+ std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
+ image_channel),
[this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
}),
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index b4d3825..f98aadb 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -35,7 +35,7 @@
std::string hostname = FLAGS_pi;
if (hostname == "") {
hostname = aos::network::GetHostname();
- LOG(INFO) << "Using pi name from hostname as " << hostname;
+ LOG(INFO) << "Using pi/orin name from hostname as " << hostname;
}
CHECK(!FLAGS_base_intrinsics.empty())
<< "Need a base intrinsics json to use to auto-capture images when the "
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index ab03e01..49224f0 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -7,7 +7,7 @@
std::string_view base_intrinsics_file, bool display_undistorted,
std::string_view calibration_folder, aos::ExitHandle *exit_handle)
: pi_(pi),
- pi_number_(aos::network::ParsePiNumber(pi)),
+ pi_number_(aos::network::ParsePiOrOrinNumber(pi)),
camera_id_(camera_id),
prev_H_camera_board_(Eigen::Affine3d()),
prev_image_H_camera_board_(Eigen::Affine3d()),
@@ -25,11 +25,13 @@
HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
rvecs_eigen, tvecs_eigen);
}),
+ // TODO: Need to make this work with /pi or /orin
image_callback_(
event_loop,
- absl::StrCat("/pi",
- std::to_string(aos::network::ParsePiNumber(pi).value()),
- "/camera"),
+ absl::StrCat(
+ "/pi",
+ std::to_string(aos::network::ParsePiOrOrinNumber(pi).value()),
+ "/camera"),
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
@@ -213,7 +215,7 @@
const aos::realtime_clock::time_point realtime_now =
aos::realtime_clock::now();
std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(pi_);
+ aos::network::team_number_internal::ParsePiOrOrinTeamNumber(pi_);
CHECK(team_number) << ": Invalid pi hostname " << pi_
<< ", failed to parse team number";
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
new file mode 100644
index 0000000..b65e883
--- /dev/null
+++ b/frc971/vision/vision_util_lib.cc
@@ -0,0 +1,46 @@
+#include "frc971/vision/vision_util_lib.h"
+
+#include "glog/logging.h"
+
+namespace frc971::vision {
+
+std::optional<cv::Mat> CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ CHECK(!camera_calibration->has_turret_extrinsics())
+ << "Turret not currently supported";
+
+ if (!camera_calibration->has_fixed_extrinsics()) {
+ return std::nullopt;
+ }
+ CHECK(camera_calibration->fixed_extrinsics()->has_data());
+ cv::Mat result(4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->fixed_extrinsics()->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(),
+ camera_calibration->fixed_extrinsics()->data()->size());
+
+ return result;
+}
+
+cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+ return result;
+}
+
+cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+}
+
+} // namespace frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
new file mode 100644
index 0000000..6fb32eb
--- /dev/null
+++ b/frc971/vision/vision_util_lib.h
@@ -0,0 +1,22 @@
+#ifndef FRC971_VISION_VISION_UTIL_LIB_H_
+#define FRC971_VISION_VISION_UTIL_LIB_H_
+#include <optional>
+#include <string_view>
+
+#include "opencv2/imgproc.hpp"
+
+#include "frc971/vision/calibration_generated.h"
+
+namespace frc971::vision {
+std::optional<cv::Mat> CameraExtrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration *camera_calibration);
+
+} // namespace frc971::vision
+
+#endif // FRC971_VISION_VISION_UTIL_LIB_H_