add pva backend

scripts/deploy.sh

@@ -26,5 +26,5 @@ while true; do
 done
 
 rsync -avz --delete bin "$HOST":/bos
-rsync -avz constants "$HOST":/bos
+rsync -avz --delete constants "$HOST":/bos
 ssh $HOST 'sudo systemctl restart bos.service'

src/camera/cv_camera.cc

@@ -33,7 +33,6 @@ CVCamera::CVCamera(const CameraConstant& c, std::optional<std::string> log_path)
   LOG(INFO) << c.pipeline.value();
 
   backup_image_ = cv::imread("/bos/constants/dont_worry_about_it.jpg");
-  LOG(INFO) << "Backup image empty: " << backup_image_.empty();
   if (c.frame_height.has_value() && c.frame_width.has_value()) {
     cv::resize(backup_image_, backup_image_,
                cv::Size(c.frame_width.value(), c.frame_height.value()));

src/localization/CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 3.10)
 
 add_library(localization position_sender.cc 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)
-target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil)
+target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi)

src/localization/nvidia_apriltag_detector.cc

@@ -2,6 +2,7 @@
 #include <fmt/chrono.h>
 #include <vpi/Array.h>
 #include <vpi/Stream.h>
+#include <vpi/algo/ConvertImageFormat.h>
 #include <Eigen/Geometry>
 #include <vpi/OpenCVInterop.hpp>
 #include "src/localization/position.h"
@@ -11,28 +12,9 @@
 
 namespace localization {
 
-auto Transform3dFromMatrix(float matrix[3][4]) -> frc::Pose3d {  // NOLINT
-  // Need to convert to wpilib coordinates
-  const float x_translation = matrix[2][3];
-  const float y_translation = matrix[0][3];
-  const float z_translation = matrix[1][3];
-
-  Eigen::Matrix3d rotation_matrix{{matrix[2][2], matrix[2][0], matrix[2][1]},
-                                  {matrix[0][2], matrix[0][0], matrix[0][1]},
-                                  {matrix[1][2], matrix[1][0], matrix[1][1]}};
-
-  // Converting to quaternion because wpilib fails to directly convert from matrix
-  Eigen::Quaterniond quaternion(rotation_matrix);
-  return {frc::Translation3d(units::meter_t{x_translation},
-                             units::meter_t{y_translation},
-                             units::meter_t{z_translation}),
-          frc::Rotation3d(frc::Quaternion(quaternion.w(), quaternion.x(),
-                                          quaternion.y(), quaternion.z()))};
-}
-
 NvidiaAprilTagDetector::NvidiaAprilTagDetector(int image_width,
                                                int image_height,
-                                               nlohmann::json intrinsics,
+                                               const nlohmann::json& intrinsics,
                                                VPIAprilTagDecodeParams params,
                                                VPIBackend backend,
                                                int max_detections, bool verbose)
@@ -41,18 +23,12 @@ NvidiaAprilTagDetector::NvidiaAprilTagDetector(int image_width,
       max_detections_(max_detections),
       input_(nullptr) {
 
-  intrinsics_[0][0] = intrinsics["fx"];
-  intrinsics_[0][2] = intrinsics["cx"];
-  intrinsics_[1][1] = intrinsics["fy"];
-  intrinsics_[1][2] = intrinsics["cy"];
-
-  (vpiCreateAprilTagDetector(backend_, image_width, image_height, &params_,
-                             &payload_));
+  CHECK(!vpiCreateAprilTagDetector(backend_, image_width, image_height,
+                                   &params_, &payload_));
 
-  (vpiArrayCreate(max_detections_, VPI_ARRAY_TYPE_APRILTAG_DETECTION, 0,
-                  &detections_));
-  (vpiArrayCreate(max_detections_, VPI_ARRAY_TYPE_POSE, 0, &poses_));
-  (vpiStreamCreate(0, &stream_));
+  CHECK(!vpiArrayCreate(max_detections_, VPI_ARRAY_TYPE_APRILTAG_DETECTION, 0,
+                        &detections_));
+  CHECK(!vpiStreamCreate(0, &stream_));
 }
 
 auto NvidiaAprilTagDetector::GetTagDetections(
@@ -67,19 +43,19 @@ auto NvidiaAprilTagDetector::GetTagDetections(
   }
 
   if (input_ == nullptr) {
-    (vpiImageCreateWrapperOpenCVMat(gray, 0, &input_));
+    CHECK(!vpiImageCreateWrapperOpenCVMat(gray, 0, &input_));
   } else {
-    (vpiImageSetWrappedOpenCVMat(input_, gray));
+    CHECK(!vpiImageSetWrappedOpenCVMat(input_, gray));
   }
 
-  (vpiSubmitAprilTagDetector(stream_, backend_, payload_, max_detections_,
-                             input_, detections_));
+  CHECK(!vpiSubmitAprilTagDetector(stream_, backend_, payload_, max_detections_,
+                                   input_, detections_));
 
-  vpiStreamSync(stream_);
+  CHECK(!vpiStreamSync(stream_));
 
   VPIArrayData detections_data{};
-  vpiArrayLockData(detections_, VPI_LOCK_READ, VPI_ARRAY_BUFFER_HOST_AOS,
-                   &detections_data);
+  CHECK(!vpiArrayLockData(detections_, VPI_LOCK_READ, VPI_ARRAY_BUFFER_HOST_AOS,
+                          &detections_data));
 
   std::vector<tag_detection_t> tag_detections;
   auto* detections =
@@ -90,9 +66,8 @@ auto NvidiaAprilTagDetector::GetTagDetections(
     tag_detection_t detection;
     detection.tag_id = detections[i].id;
     detection.timestamp = timestamped_frame.timestamp;
-    detection.confidence = detections[i].decisionMargin;
+    detection.confidence = 1;
 
-    // Extract corners from VPI detection
     for (int j = 0; j < 4; ++j) {
       detection.corners[j] =
           cv::Point2f(detections[i].corners[j].x, detections[i].corners[j].y);
@@ -101,14 +76,14 @@ auto NvidiaAprilTagDetector::GetTagDetections(
     tag_detections.push_back(detection);
   }
 
-  vpiArrayUnlock(detections_);
+  CHECK(!vpiArrayUnlock(detections_));
   return tag_detections;
 }
 
 NvidiaAprilTagDetector::~NvidiaAprilTagDetector() {
   vpiStreamDestroy(stream_);
   vpiArrayDestroy(detections_);
-  vpiArrayDestroy(poses_);
+  vpiImageDestroy(input_);
   vpiPayloadDestroy(payload_);
 }
 }  // namespace localization

src/localization/nvidia_apriltag_detector.h

@@ -10,12 +10,14 @@ namespace localization {
 // Nvidia's apriltag detection algorithim
 // Supports either VPA or CPU backend
 // VPA and CPU are both slower than GPU we learned through benchmarks, but they can be used to offload some computation from the GPU
-class NvidiaAprilTagDetector : IAprilTagDetector {
+class NvidiaAprilTagDetector : public IAprilTagDetector {
  public:
-  NvidiaAprilTagDetector(int image_width, int image_height,
-                         nlohmann::json intrinsics,
-                         VPIAprilTagDecodeParams params, VPIBackend backend,
-                         int max_detections = 16, bool verbose = false);
+  NvidiaAprilTagDetector(
+      int image_width, int image_height, const nlohmann::json& intrinsics,
+      VPIAprilTagDecodeParams params = {NULL, 0, 1,  // NOLINT
+                                        VPIAprilTagFamily::VPI_APRILTAG_36H11},
+      VPIBackend backend = VPIBackend::VPI_BACKEND_PVA,
+      int max_detections = 128, bool verbose = false);
   ~NvidiaAprilTagDetector() override;
   auto GetTagDetections(camera::timestamped_frame_t& frame)
       -> std::vector<tag_detection_t> override;
@@ -25,10 +27,8 @@ class NvidiaAprilTagDetector : IAprilTagDetector {
   VPIBackend backend_;
   int max_detections_;
   VPIImage input_;
-  VPICameraIntrinsic intrinsics_;
   VPIPayload payload_;
   VPIArray detections_;
-  VPIArray poses_;
   VPIStream stream_;
 };
 }  // namespace localization

src/localization/square_solver.cc

@@ -47,6 +47,11 @@ auto SquareSolver::EstimatePosition(
   std::vector<position_estimate_t> position_estimates;
   position_estimates.reserve(detections.size());
   for (const auto& detection : detections) {
+    if (!localization::kapriltag_layout.GetTagPose(detection.tag_id)
+             .has_value()) {
+      LOG(WARNING) << "Got invalid tag";
+      continue;
+    }
     if (reject_far_tags) {
       const auto& c = detection.corners;
       const double area = 0.5 * std::abs((c[0].x - c[2].x) * (c[1].y - c[3].y) -

src/main_bot_main.cc

@@ -2,6 +2,7 @@
 #include "src/camera/camera_source.h"
 #include "src/camera/cv_camera.h"
 #include "src/localization/multi_tag_solver.h"
+#include "src/localization/nvidia_apriltag_detector.h"
 #include "src/localization/opencv_apriltag_detector.h"
 #include "src/localization/run_localization.h"
 #include "src/localization/square_solver.h"
@@ -37,7 +38,7 @@ auto main() -> int {
 
   std::thread front_thread(
       localization::RunLocalization, std::ref(front_camera),
-      std::make_unique<localization::OpenCVAprilTagDetector>(
+      std::make_unique<localization::NvidiaAprilTagDetector>(
           front_camera.GetFrame().cols, front_camera.GetFrame().rows,
           utils::ReadIntrinsics(
               camera_constants.at("main_bot_front").intrinsics_path.value())),