[wpical] Refactor to use WPILib libraries and modern C++ conventions and improve UX (#7796)

wpical was unable to use wpimath and its dependent libraries because
Ceres was compiled with a different version of Eigen. Now that the Ceres
build has been redone and shipped in #8151, we can now use wpimath and
our C++ apriltag wrapper in wpical, allowing for major refactors. This
includes:
* Using `to_json` and `from_json` specializations to concisely serialize
and deserialize all JSON files instead of manually handling JSON.
* Removal of the `Fieldmap` and `Pose` classes, which were duplicates of
the `AprilTagFieldLayout` and `Pose3d` classes respectively.
* Using `AprilTagDetector` instead of the raw libapriltag library.
* Using `Pose3d` instead of raw Eigen matrices.

In addition, several other refactors were made to make the code more
readable and to fix several UX issues and crashes. This includes:
* Eagerly parsing every JSON file when selected by the user. This means
JSON files are only parsed once on selection, instead of every time a
downstream function wants to use the data. This also means invalid JSON
can be detected upfront and a specific error shown immediately instead
of a catch all error when trying to calibrate.
* Using `std::optional` to indicate a calibration failed instead of
status codes.
* Processing videos on separate threads to not block the UI thread and
take advantage of parallelization for camera calibration. (2x speedup on
my laptop)
* Removing the OpenCV calibration option, since mrcal should be better
in every scenario.
* Showing a progress bar for camera calibration.
* Breaking up the massive `DisplayGui` function into separate functions
which contain code for different popups. This also allowed for better
organization and scoping of static variables.
* Renaming variables to make their purpose more clear.
* Displaying the tags present in a field layout when trying to combine
multiple field layouts.

Fixes #7722.

tools/wpical/src/test/native/cpp/test_calibrate.cpp

@@ -2,6 +2,8 @@
 // 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 <fstream>
+#include <optional>
 #include <string>
 
 #include <gtest/gtest.h>
@@ -9,6 +11,8 @@
 #include "cameracalibration.hpp"
 #include "fieldcalibration.hpp"
 #include "path_lookup.hpp"
+#include "wpi/apriltag/AprilTagFieldLayout.hpp"
+#include "wpi/apriltag/AprilTagFields.hpp"
 #include "wpi/util/json.hpp"
 
 const std::string projectRootPath = PROJECT_ROOT_PATH;
@@ -21,12 +25,6 @@ const std::string calSavePath =
                0, projectRootPath.find("/src/main/native/assets")) +
            "/build")
         : std::string(tmpdir_c_str);
-cameracalibration::CameraModel cameraModel = {
-    .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
-    .distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
-    .avg_reprojection_error = 0.0};
-
-wpi::util::json output_json;
 
 #ifdef __linux__
 const std::string fileSuffix = ".avi";
@@ -36,80 +34,93 @@ const std::string fileSuffix = ".mp4";
 const std::string videoLocation = "/fieldvideo";
 #endif
 
-TEST(Camera_CalibrationTest, OpenCV_Typical) {
-  int ret = cameracalibration::calibrate(
-      LookupPath(projectRootPath + "/testcalibration" + fileSuffix),
-      cameraModel, 0.709f, 0.551f, 12, 8, false);
-  cameracalibration::dumpJson(cameraModel,
-                              calSavePath + "/cameracalibration.json");
-  EXPECT_EQ(ret, 0);
+TEST(CameraCalibrationTest, Typical) {
+  auto path = LookupPath(projectRootPath + "/testcalibration" + fileSuffix);
+  auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path);
+  while (!calibrator.IsFinished()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  auto ret = calibrator.GetCameraModel();
+  EXPECT_NE(ret, std::nullopt);
+  std::ofstream output_file(calSavePath + "/cameracalibration.json");
+  output_file << wpi::util::json(ret.value()).dump(4) << std::endl;
 }
 
-TEST(Camera_CalibrationTest, OpenCV_Atypical) {
-  int ret = cameracalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix),
-      cameraModel, 0.709f, 0.551f, 12, 8, false);
-  EXPECT_EQ(ret, 1);
+TEST(CameraCalibrationTest, Atypical) {
+  auto path =
+      LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix);
+  auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path);
+  while (!calibrator.IsFinished()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  auto ret = calibrator.GetCameraModel();
+  EXPECT_EQ(ret, std::nullopt);
 }
 
-TEST(Camera_CalibrationTest, MRcal_Typical) {
-  int ret = cameracalibration::calibrate(
-      LookupPath(projectRootPath + "/testcalibration" + fileSuffix),
-      cameraModel, .709f, 12, 8, 1080, 1920, false);
-  EXPECT_EQ(ret, 0);
+TEST(FieldCalibrationTest, Typical) {
+  auto model = wpi::util::json::parse(
+                   std::ifstream(calSavePath + "/cameracalibration.json"))
+                   .get<wpical::CameraModel>();
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
+                        wpi::apriltag::AprilTagFieldLayout::LoadField(
+                            wpi::apriltag::AprilTagField::k2024Crescendo),
+                        3, false);
+  EXPECT_NE(ret, std::nullopt);
 }
 
-TEST(Camera_CalibrationTest, MRcal_Atypical) {
-  int ret = cameracalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix),
-      cameraModel, 0.709f, 12, 8, 1080, 1920, false);
-  EXPECT_EQ(ret, 1);
+TEST(FieldCalibrationTest, Atypical_Bad_Camera_Model) {
+  wpical::CameraModel model{};
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
+                        wpi::apriltag::AprilTagFieldLayout::LoadField(
+                            wpi::apriltag::AprilTagField::k2024Crescendo),
+                        3, false);
+  EXPECT_EQ(ret, std::nullopt);
 }
 
-TEST(Field_CalibrationTest, Typical) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation), output_json,
-      calSavePath + "/cameracalibration.json",
-      LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
-  EXPECT_EQ(ret, 0);
+TEST(FieldCalibrationTest, Atypical_Bad_Field_Layout) {
+  auto model = wpi::util::json::parse(
+                   std::ifstream(calSavePath + "/cameracalibration.json"))
+                   .get<wpical::CameraModel>();
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
+                        wpi::apriltag::AprilTagFieldLayout{}, 3, false);
+  EXPECT_EQ(ret, std::nullopt);
 }
 
-TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation), output_json,
-      LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix),
-      LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
-  EXPECT_EQ(ret, 1);
+TEST(FieldCalibrationTest, Atypical_Bad_Input_Directory) {
+  auto model = wpi::util::json::parse(
+                   std::ifstream(calSavePath + "/cameracalibration.json"))
+                   .get<wpical::CameraModel>();
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath), model,
+                        wpi::apriltag::AprilTagFieldLayout::LoadField(
+                            wpi::apriltag::AprilTagField::k2024Crescendo),
+                        3, false);
+  EXPECT_EQ(ret, std::nullopt);
 }
 
-TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation), output_json,
-      calSavePath + "/cameracalibration.json",
-      calSavePath + "/cameracalibration.json", 3, false);
-  EXPECT_EQ(ret, 1);
+TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag) {
+  auto model = wpi::util::json::parse(
+                   std::ifstream(calSavePath + "/cameracalibration.json"))
+                   .get<wpical::CameraModel>();
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
+                        wpi::apriltag::AprilTagFieldLayout::LoadField(
+                            wpi::apriltag::AprilTagField::k2024Crescendo),
+                        42, false);
+  EXPECT_EQ(ret, std::nullopt);
 }
 
-TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + ""), output_json,
-      calSavePath + "/cameracalibration.json",
-      LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
-  EXPECT_EQ(ret, 1);
-}
-
-TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation), output_json,
-      calSavePath + "/cameracalibration.json",
-      LookupPath(projectRootPath + "/2024-crescendo.json"), 42, false);
-  EXPECT_EQ(ret, 1);
-}
-
-TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
-  int ret = fieldcalibration::calibrate(
-      LookupPath(projectRootPath + videoLocation), output_json,
-      calSavePath + "/cameracalibration.json",
-      LookupPath(projectRootPath + "/2024-crescendo.json"), -1, false);
-  EXPECT_EQ(ret, 1);
+TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
+  auto model = wpi::util::json::parse(
+                   std::ifstream(calSavePath + "/cameracalibration.json"))
+                   .get<wpical::CameraModel>();
+  auto ret =
+      wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
+                        wpi::apriltag::AprilTagFieldLayout::LoadField(
+                            wpi::apriltag::AprilTagField::k2024Crescendo),
+                        -1, false);
+  EXPECT_EQ(ret, std::nullopt);
 }