Create y2024_bot3 folder
Change-Id: Iee232bde58c0425920a49eee145a0f93c7485391
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
Signed-off-by: James (Peilun) Li <jamespeilunli@gmail.com>
diff --git a/y2024_bot3/vision/vision_util.cc b/y2024_bot3/vision/vision_util.cc
new file mode 100644
index 0000000..0f99849
--- /dev/null
+++ b/y2024_bot3/vision/vision_util.cc
@@ -0,0 +1,49 @@
+#include "y2024_bot3/vision/vision_util.h"
+
+#include "absl/log/check.h"
+#include "absl/log/log.h"
+
+namespace y2024_bot3::vision {
+
+// Store a list of ordered cameras as you progress around the robot/box of orins
+std::vector<CameraNode> CreateNodeList() {
+ std::vector<CameraNode> list;
+
+ list.push_back({.node_name = "imu", .camera_number = 0});
+ list.push_back({.node_name = "imu", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 1});
+ list.push_back({.node_name = "orin1", .camera_number = 0});
+
+ return list;
+}
+
+// From the node_list, create a numbering scheme from 0 to 3
+std::map<std::string, int> CreateOrderingMap(
+ std::vector<CameraNode> &node_list) {
+ std::map<std::string, int> map;
+
+ for (uint i = 0; i < node_list.size(); i++) {
+ map.insert({node_list.at(i).camera_name(), i});
+ }
+
+ return map;
+}
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2024_bot3::Constants &calibration_data, std::string_view node_name,
+ int camera_number) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2024_bot3::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ 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
+ << " and camera number " << camera_number;
+}
+
+} // namespace y2024_bot3::vision