Joint solve (but better)

constants/camera_constants.json

@@ -20,7 +20,16 @@
       "pipeline": "/dev/v4l/by-path/platform-3610000.usb-usb-0:2.1:1.0-video-index0",
       "intrinsics_path": "/bos/constants/misc/dev_orin_intrinsics.json",
       "extrinsics_path": "/bos/constants/misc/dev_orin_extrinsics.json",
-      "name": "dev_orin"
+      "name": "dev_orin",
+      "backlight": null,
+      "frame_width": 1280,
+      "frame_height": 800,
+      "fps": 100.0,
+      "exposure": null,
+      "port": 5801,
+      "detector_type": "austin_gpu",
+      "serial_id": "devorin",
+      "streamer_fps": 1
     },
     {
       "name": "main_bot_front",
@@ -101,4 +110,4 @@
       "detector_type": "austin_gpu"
     }
   ]
-}
+}

constants/misc/dummy_camera_intrinsics.json

@@ -1,12 +1,11 @@
 {
-    "cx": 10.0,
-    "cy": 10.0,
-    "fx": 1.0,
-    "fy": 1.0,
-    "k1": 0.0,
-    "k2": 0.0,
-    "k3": 0.0,
-    "p1": 0.0,
-    "p2": 0.0
+    "cx": 669.6571418509437,
+    "cy": 386.5080452595225,
+    "fx": 898.443795092944,
+    "fy": 898.4888081021497,
+    "k1": 0,
+    "k2": 0,
+    "k3": 0,
+    "p1": 0,
+    "p2": 0
 }
-

constants/stovetop_bot/front_right_extrinsics.json

@@ -0,0 +1,8 @@
+{
+  "translation_x": -0.313,
+  "translation_y": 0.28,
+  "translation_z": 0.182,
+  "rotation_x": 0,
+  "rotation_y": -0.1717,
+  "rotation_z": 3.55
+}

constants/stovetop_bot/front_right_intrinsics.json

@@ -0,0 +1,12 @@
+{
+    "cx": 1012.0165186318737,
+    "cy": 509.96268009601044,
+    "fx": 598.8562147696354,
+    "fy": 598.7791606886024,
+    "k1": 0.01265280096846185,
+    "k2": -0.02749231422823394,
+    "k3": 0.004217234226796008,
+    "p1": 6.344713648157099e-05,
+    "p2": -0.0015890075748726767
+}
+

src/camera/camera_constants.cc

@@ -82,6 +82,8 @@ auto GetCameraConstants(const std::string& path) -> camera_constants_t {
     SetConstant<double>("stream_ratio", camera_constant.stream_ratio,
                         camera_config);
     SetConstant<uint>("port", camera_constant.port, camera_config);
+    SetConstant<uint>("streamer_fps", camera_constant.streamer_fps,
+                      camera_config);
 
     if (camera_config.contains("detector_type") &&
         !camera_config["detector_type"].is_null()) {

src/camera/camera_constants.h

@@ -24,8 +24,13 @@ using camera_constant_t = struct CameraConstant {
   std::optional<std::string> serial_id = std::nullopt;      // uvc only
   std::optional<double> stream_ratio = std::nullopt;
   std::optional<uint> port = std::nullopt;
+  std::optional<uint> streamer_fps = std::nullopt;
   DetectorType detector_type = INVALID;
 
+  auto operator==(const CameraConstant& other) const -> bool {
+    return name == other.name;
+  }
+
   friend auto operator<<(std::ostream& os, const CameraConstant& c)
       -> std::ostream& {
     os << "pipeline: " << c.pipeline.value_or("NO PIPELINE VALUE")

src/camera/cv_camera.cc

@@ -66,7 +66,7 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional<std::string> log_path)
 auto CVCamera::GetFrame() -> timestamped_frame_t {
   timestamped_frame_t timestamped_frame;
   cv::Mat raw_image;
-  if (!cap_.grab()) {
+  while (!cap_.grab()) {
     Restart();
     LOG(WARNING) << "Restarting camera";
   }
@@ -76,7 +76,7 @@ auto CVCamera::GetFrame() -> timestamped_frame_t {
   raw_image.copyTo(timestamped_frame.frame);
 
   if (timestamped_frame.frame.empty()) {
-    timestamped_frame.frame = backup_image_;
+    timestamped_frame.invalid = true;
   }
   if (timestamped_frame.frame.channels() == 4) {
     cv::cvtColor(timestamped_frame.frame, timestamped_frame.frame,

src/camera/disk_camera.cc

@@ -23,12 +23,6 @@ DiskCamera::DiskCamera(std::string image_folder_path,
 
     double timestamp = std::stod(entry_name.erase(entry_name.size() - 4, 4));
 
-    if (start.has_value() && timestamp < start_) {
-      continue;
-    }
-    if (end.has_value() && timestamp > end_) {
-      continue;
-    }
     // remove .png for timestamp
     image_paths_.push(
         timestamped_frame_path_t{.path = entry.path(), .timestamp = timestamp});
@@ -51,6 +45,12 @@ DiskCamera::DiskCamera(std::string image_folder_path,
     }
     image_paths_.pop();
     entry.timestamp -= offset;
+    if (start.has_value() && entry.timestamp < start_) {
+      continue;
+    }
+    if (end.has_value() && entry.timestamp > end_) {
+      continue;
+    }
     normalized.push(entry);
   }
   image_paths_ = std::move(normalized);
@@ -59,7 +59,6 @@ auto DiskCamera::GetFrame() -> timestamped_frame_t {
   if (image_paths_.empty()) {
     std::cout << "Finished reading all frames from DiskCamera. Folder path: "
               << image_folder_path_ << std::endl;
-    frc::DataLogManager::Stop();
     return {.invalid = true};
   }
 

src/localization/CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
 
 add_library(localization gpu_apriltag_detector.cc opencv_apriltag_detector.cc run_localization.cc nvidia_apriltag_detector.cc square_solver.cc joint_solver.cc multi_tag_solver.cc position_receiver.cc unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc)
-target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status)
+target_sources(localization PRIVATE multi_camera_detector.cc)
+target_link_libraries(localization PUBLIC ${OpenCV_LIBS} 971apriltag utils camera wpilibc wpiutil vpi absl::status)

src/localization/README.md

@@ -0,0 +1,77 @@
+# Localization Module
+
+We use AprilTags for localization.
+
+---
+
+## Base Interface
+
+### `position_solver.h`
+Interface for converting tag detections into robot pose estimates.
+
+---
+
+## Implementations
+
+### `square_solver.{cc,h}`
+Uses OpenCV's IPPE_SQUARE solver. Takes advantage of the fact that all object points are planar and a set distance and angle away from each other, so produces the best estimates for individual tag detections.
+
+### `multi_tag_solver.{cc,h}`
+Uses in multiple tags at a time, uses a different OpenCV algorithm because the points aren't all planar anymore once we pass in multiple tags. Can only be run on a per-camera basis because OpenCV limits what kinds of matrices you can pass into its functions, so we didn't figure out how to solve using all detections from all cameras at once using OpenCV.
+
+### `joint_solver.{cc,h}`
+Custom iterative solver using multiple tag estimates.
+
+The normal PnP problem is:
+
+```
+
+image_point · scalar = image_to_camera · camera_to_world · world_relative_points;
+
+```
+
+In our scenario this is:
+
+```
+
+image_point · scalar = image_to_camera · camera_to_tag · tag_to_field · tag_center_to_tag_corner;
+
+```
+
+This can be rewritten as:
+
+```
+
+image_point · scalar = image_to_camera · camera_to_robot · robot_to_field · field_to_tag · tag_center_to_tag_corner;
+
+```
+
+And simplified to:
+
+```
+
+image_point · scalar = image_to_robot · robot_to_field · field_relative_tag_corner;
+
+```
+
+For an iterative solver, you would normally just compute the gradient here, but if you directly applied the gradient update to the rotation matrix, it wouldn't be guaranteed to be a rotation matrix. We therefore split up the transformation matrix robot_to_field into translation and individual Euler angles and update with respect to those variables instead.
+
+After decomposition the math is effectively:
+
+```
+
+image_to_robot · translation · rotation_z · rotation_y · rotation_x · field_to_tag_center · tag_center_to_tag_corner = projected_corner_in_image
+
+```
+
+We treat like it is a neural network, with the layers being:
+
+```
+
+input/object_point → rotation_x → rotation_y → rotation_z → translation → image_to_robot = projection
+
+```
+
+If image_to_robot being at the end seems confusing, from_image_to_robot really means the robot's position expressed in image coordinates, meaning that multiplying image_to_robot by a matrix in robot coordinates takes it from robot coordinates to image coordinates (the projection).
+```
+