Mostly cosmetic changes to the code
Rename "pi" to "orin" or "node" or "camera" as appropriate
Moved data structures for managing cameras and ordering to shared library
Added some visualization of the extrinsics in calibration and mapping
Change-Id: I6f9e1badaa4b88aff76aecd1c572d54265dd7578
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2024/vision/vision_util.cc b/y2024/vision/vision_util.cc
index 6acd234..becb1cf 100644
--- a/y2024/vision/vision_util.cc
+++ b/y2024/vision/vision_util.cc
@@ -4,6 +4,30 @@
namespace y2024::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::Constants &calibration_data, std::string_view node_name,
int camera_number) {