Enable support for cancelling mrcal operations in wpical.

tools/wpical/src/main/native/cpp/WPIcal.cpp

@@ -2,8 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <chrono>
 #include <filesystem>
 #include <format>
+#include <future>
 #include <map>
 #include <memory>
 #include <optional>
@@ -284,8 +286,25 @@ void CalibrateCamera() {
   static std::unique_ptr<pfd::open_file> cameraVideoSelector;
   static std::string cameraVideoPath;
   static std::unique_ptr<wpical::CameraCalibrator> videoProcessor;
+  static std::vector<std::future<void>> videoProcessorCleanupTasks;
   static bool calibrating = false;
 
+  std::erase_if(videoProcessorCleanupTasks, [](auto& task) {
+    return task.wait_for(std::chrono::seconds{0}) == std::future_status::ready;
+  });
+  const bool videoProcessorCleanupPending = !videoProcessorCleanupTasks.empty();
+
+  auto cleanupVideoProcessor = [&] {
+    if (!videoProcessor) {
+      return;
+    }
+    videoProcessor->Stop();
+    videoProcessorCleanupTasks.emplace_back(std::async(
+        std::launch::async, [processor = std::move(videoProcessor)]() mutable {
+          processor.reset();
+        }));
+  };
+
   static double squareWidth = 0.709;
   static double markerWidth = 0.551;
   static int boardWidth = 12;
@@ -338,6 +357,7 @@ void CalibrateCamera() {
         }
         ImGui::CloseCurrentPopup();
         calibrating = false;
+        cleanupVideoProcessor();
       } else if (!videoProcessor->IsFinished()) {
         double processed = videoProcessor->TotalFramesProcessed();
         double total = videoProcessor->TotalFrames();
@@ -354,21 +374,28 @@ void CalibrateCamera() {
         ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
         ImGui::OpenPopup("Camera Calibration Error");
         calibrating = false;
+        cleanupVideoProcessor();
       }
-    } else if (ImGui::Button("Calibrate") && !cameraVideoPath.empty()) {
-      videoProcessor = std::make_unique<wpical::CameraCalibrator>(
-          numWorkers, squareWidth, markerWidth, boardWidth, boardHeight,
-          cameraVideoPath);
-      calibrating = true;
+    } else {
+      ImGui::BeginDisabled(videoProcessorCleanupPending);
+      if (ImGui::Button("Calibrate") && !cameraVideoPath.empty()) {
+        videoProcessor = std::make_unique<wpical::CameraCalibrator>(
+            numWorkers, squareWidth, markerWidth, boardWidth, boardHeight,
+            cameraVideoPath);
+        calibrating = true;
+      }
+      ImGui::EndDisabled();
     }
     ImGui::SameLine();
     if (ImGui::Button("Close")) {
-      if (videoProcessor) {
-        videoProcessor->Stop();
-      }
+      cleanupVideoProcessor();
       calibrating = false;
       ImGui::CloseCurrentPopup();
     }
+    if (videoProcessorCleanupPending) {
+      ImGui::TextUnformatted(
+          "Waiting for the previous calibration to finish...");
+    }
     ImGui::EndPopup();
   }
 }

tools/wpical/src/main/native/cpp/cameracalibration.cpp

@@ -80,7 +80,7 @@ class Data {
    */
   std::optional<cv::Mat> GetFrame();
 
-  std::atomic_bool m_isFinished;
+  std::atomic_bool m_isFinished{false};
   std::optional<CameraModel> cameraModel;
   std::queue<cv::Mat> queue;
   wpi::util::mutex mutex;
@@ -232,48 +232,64 @@ CameraCalibrator::CameraCalibrator(size_t numWorkers, double squareWidth,
   }
   cv::VideoCapture cap{videoPath};
   m_totalFrames = cap.get(cv::CAP_PROP_FRAME_COUNT);
-  std::thread([this, boardHeight, boardWidth, squareWidth, state = m_state,
-               capture = std::move(cap)]() mutable {
-    cv::Size frameShape{
-        static_cast<int>(capture.get(cv::CAP_PROP_FRAME_WIDTH)),
-        static_cast<int>(capture.get(cv::CAP_PROP_FRAME_HEIGHT))};
-    while (capture.grab() && !state->m_isFinished) {
-      cv::Mat frame;
-      capture.retrieve(frame);
-      cv::Mat frameGray;
-      cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY);
-      state->AddFrame(std::move(frameGray));
-    }
-    while (!state->m_isFinished && TotalFramesProcessed() < TotalFrames()) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
-    if (state->m_isFinished) {
-      state->cameraModel = std::nullopt;
-      return;
-    }
-    std::vector<mrcal_point3_t> allObservationBoards;
-    std::vector<mrcal_pose_t> allFramesRtToref;
-    for (auto& worker : m_workers) {
-      auto data = worker->GetData();
-      allObservationBoards.insert(allObservationBoards.end(),
-                                  data.first.begin(), data.first.end());
-      allFramesRtToref.insert(allFramesRtToref.end(), data.second.begin(),
-                              data.second.end());
-    }
-    if (allObservationBoards.empty()) {
-      state->m_isFinished = true;
-      return;
-    }
-    auto result = mrcal_main(allObservationBoards, allFramesRtToref,
-                             cv::Size(boardWidth - 1, boardHeight - 1),
-                             squareWidth * 0.0254, frameShape, 1000);
-    state->cameraModel = MrcalResultToCameraModel(*result);
-    state->m_isFinished = true;
-  }).detach();
+  m_processingThread =
+      std::thread([this, boardHeight, boardWidth, squareWidth, state = m_state,
+                   capture = std::move(cap),
+                   stopToken = m_stopSource.get_token()]() mutable {
+        cv::Size frameShape{
+            static_cast<int>(capture.get(cv::CAP_PROP_FRAME_WIDTH)),
+            static_cast<int>(capture.get(cv::CAP_PROP_FRAME_HEIGHT))};
+        int framesQueued = 0;
+        while (capture.grab() && !state->m_isFinished) {
+          cv::Mat frame;
+          if (!capture.retrieve(frame) || frame.empty()) {
+            continue;
+          }
+          cv::Mat frameGray;
+          cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY);
+          state->AddFrame(std::move(frameGray));
+          ++framesQueued;
+        }
+        m_totalFrames = framesQueued;
+        while (!state->m_isFinished && TotalFramesProcessed() < framesQueued) {
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+        if (state->m_isFinished) {
+          state->cameraModel = std::nullopt;
+          return;
+        }
+        std::vector<mrcal_point3_t> allObservationBoards;
+        std::vector<mrcal_pose_t> allFramesRtToref;
+        for (auto& worker : m_workers) {
+          auto data = worker->GetData();
+          allObservationBoards.insert(allObservationBoards.end(),
+                                      data.first.begin(), data.first.end());
+          allFramesRtToref.insert(allFramesRtToref.end(), data.second.begin(),
+                                  data.second.end());
+        }
+        if (allObservationBoards.empty()) {
+          state->m_isFinished = true;
+          return;
+        }
+        auto result =
+            mrcal_main(allObservationBoards, allFramesRtToref,
+                       cv::Size(boardWidth - 1, boardHeight - 1),
+                       squareWidth * 0.0254, frameShape, 1000, stopToken);
+        if (!result) {
+          state->cameraModel = std::nullopt;
+          state->m_isFinished = true;
+          return;
+        }
+        state->cameraModel = MrcalResultToCameraModel(*result);
+        state->m_isFinished = true;
+      });
 }
 
 CameraCalibrator::~CameraCalibrator() {
   Stop();
+  if (m_processingThread.joinable()) {
+    m_processingThread.join();
+  }
 }
 
 bool CameraCalibrator::IsFinished() {
@@ -297,6 +313,7 @@ int CameraCalibrator::TotalFrames() {
 }
 
 void CameraCalibrator::Stop() {
+  m_stopSource.request_stop();
   m_state->m_isFinished = true;
   for (auto& workers : m_workers) {
     workers->Stop();

tools/wpical/src/main/native/include/cameracalibration.hpp

@@ -8,6 +8,7 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <Eigen/Core>
@@ -90,6 +91,8 @@ class CameraCalibrator {
 
   std::atomic_int m_totalFrames;
   std::vector<std::shared_ptr<Worker>> m_workers;
+  mrcal_cancellation_source m_stopSource;
+  std::thread m_processingThread;
 };
 
 void to_json(wpi::util::json& json, const CameraModel& cameraModel);

tools/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h

@@ -45,6 +45,9 @@ typedef void (dogleg_callback_dense_products_t)(// in
                                                 // context
                                                 void*           cookie);
 
+// Return true to stop an optimization at the next iteration boundary
+typedef bool (dogleg_cancel_callback_t)(void* cookie);
+
 // an operating point of the solver
 typedef struct
 {
@@ -284,6 +287,9 @@ double dogleg_optimize2(double* p, unsigned int Nstate,
                         unsigned int Nmeas, unsigned int NJnnz,
                         dogleg_callback_t* f, void* cookie,
                         const dogleg_parameters2_t* parameters,
+                        dogleg_cancel_callback_t* is_cancelled,
+                        void* cancellation_cookie,
+                        bool* cancelled,
                         dogleg_solverContext_t** returnContext);
 
 // Main optimization function if we're using dense linear algebra. The arguments