Improve solver test

.vscode/launch.json

@@ -0,0 +1,37 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Debug BOS (CMake)",
+            "type": "cppdbg",
+            "request": "launch",
+            // 🔴 CHANGE THIS to your actual executable
+            "program": "${workspaceFolder}/build/src/test/solver_test",
+            "args": [],
+            "stopAtEntry": false,
+            "preLaunchTask": "build-bos",
+            "cwd": "${workspaceFolder}",
+            "environment": [
+                {
+                    "name": "LD_LIBRARY_PATH",
+                    "value": "/usr/local/cuda/lib64:${env:LD_LIBRARY_PATH}"
+                }
+            ],
+            "externalConsole": false,
+            "MIMode": "gdb",
+            "miDebuggerPath": "/usr/bin/gdb",
+            "setupCommands": [
+                {
+                    "description": "Enable pretty-printing",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ],
+            "logging": {
+                "engineLogging": false,
+                "trace": false,
+                "traceResponse": false
+            }
+        }
+    ]
+}

.vscode/tasks.json

@@ -0,0 +1,19 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "build-bos",
+            "type": "shell",
+            "command": "${workspaceFolder}/scripts/build.sh",
+            "args": [],
+            "options": {
+                "cwd": "${workspaceFolder}"
+            },
+            "problemMatcher": [],
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            }
+        }
+    ]
+}

CMakeLists.txt

@@ -1,7 +1,9 @@
 cmake_minimum_required(VERSION 3.10)
 project(BOS CXX C CUDA)
 
-set(CMAKE_BUILD_TYPE Release)
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Debug)
+endif()
 set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_CXX_STANDARD_REQUIRED True)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

constants/dummy_camera_extrinsics.json

@@ -1,8 +1,8 @@
 {
-  "translation_x": 1.0,
-  "translation_y": 2.0,
-  "translation_z": 3.0,
-  "rotation_x": 0.1,
-  "rotation_y": 0.2,
-  "rotation_z": 0.3
+  "translation_x": 0.0,
+  "translation_y": 0.0,
+  "translation_z": 0.0,
+  "rotation_x": 0.0,
+  "rotation_y": 0.0,
+  "rotation_z": 0.0
 }

scripts/build.sh

@@ -1,6 +1,14 @@
 git submodule init 
 git submodule update 
-cmake -DENABLE_CLANG_TIDY=OFF -B build -G Ninja . 
+cmake -S . -B build -G Ninja \
+  -DCMAKE_BUILD_TYPE=Debug \
+  -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \
+  -DENABLE_CLANG_TIDY=OFF \
+  -DABSL_BUILD_TESTING=OFF \
+  -DCMAKE_CXX_FLAGS_DEBUG="-O0 -g -fno-inline" \
+  -DCMAKE_C_FLAGS_DEBUG="-O0 -g -fno-inline"
 cmake --build build
 mkdir -p /bos 
-cp -r constants /bos
+if [ "$(pwd -P)/constants" != "/bos/constants" ]; then
+  cp -r constants /bos
+fi

src/localization/position.h

@@ -35,6 +35,19 @@ using position_estimate_t = struct PositionEstimate {
                units::degree_t{r.Z()}.value());
     return os;
   }
+  friend auto operator==(const PositionEstimate& left,
+                         const PositionEstimate& right) -> bool {
+    const auto& lt = left.pose.Translation();
+    const auto& lr = left.pose.Rotation();
+    const auto& rt = left.pose.Translation();
+    const auto& rr = left.pose.Rotation();
+
+    return lt.X().value() == rt.X().value() &&
+           lt.Y().value() == rt.Y().value() &&
+           lt.Z().value() == rt.Z().value() &&
+           lr.X().value() == rr.X().value() &&
+           lr.Y().value() == rr.Y().value() && lr.Z().value() == rr.Z().value();
+  }
 };
 
 using pose2d_t = struct Pose2d {

src/test/CMakeLists.txt

@@ -1,3 +1,9 @@
+include(FetchContent)
+FetchContent_Declare(
+  googletest
+  URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip
+)
+
 add_executable(apriltag_detect_test apriltag_detect_test.cc)
 target_link_libraries(apriltag_detect_test PRIVATE ${OpenCV_LIBS} 971apriltag apriltag cameraserver ntcore cscore wpiutil camera nlohmann_json::nlohmann_json Eigen3::Eigen localization absl::flags absl::flags_parse)
 
@@ -25,9 +31,9 @@ target_link_libraries(gamepiece_test PRIVATE gamepiece yolo camera utils localiz
 add_executable(gamepiece_test_no_img gamepiece_test_no_img.cc)
 target_link_libraries(gamepiece_test_no_img PRIVATE gamepiece yolo camera utils localization)
 
-add_executable(absl_flag_test absl_flag_test.cc)
-target_link_libraries(absl_flag_test absl::flags absl::flags_parse)
+add_executable(absl_flag_test2 absl_flag_test.cc)
+target_link_libraries(absl_flag_test2 absl::flags absl::flags_parse)
 
 add_executable(solver_test solver_test.cc)
-target_link_libraries(solver_test absl::flags absl::flags_parse utils localization)
+target_link_libraries(solver_test absl::flags absl::flags_parse utils localization gtest gtest_main)
 

src/test/solver_test.cc

@@ -1,34 +1,85 @@
+#include <gtest/gtest.h>
+#include "src/localization/position.h"
+#include "src/localization/position_solver.h"
 #include "src/localization/square_solver.h"
 #include "src/utils/camera_utils.h"
 #include "src/utils/intrinsics_from_json.h"
+
+constexpr double ERROR_MARGIN_TRANSLATION = 0.01;
+constexpr double ERROR_MARGIN_EULER_ANGLE = 0.01745;  // 1 deg
+
+namespace frc {
+void PrintTo(const frc::Pose3d& pose, std::ostream* os) {
+  *os << "Pose{"
+      << "t = [" << pose.Translation().X().value() << ", "
+      << pose.Translation().Y().value() << ", "
+      << pose.Translation().Z().value() << "], "
+      << "R = [" << pose.Rotation().X().value() << ", "
+      << pose.Rotation().Y().value() << ", " << pose.Rotation().Z().value()
+      << "]"
+      << "}";
+}
+
+auto wrap_compare(double angle_diff) -> double {
+  double diff = std::abs(angle_diff);
+  if (std::abs(diff - 2 * std::numbers::pi) < ERROR_MARGIN_EULER_ANGLE) {
+    diff = 0;
+  }
+  return diff;
+}
+
+inline auto operator==(const localization::position_estimate_t& lhs,
+                       const localization::position_estimate_t& rhs) -> bool {
+  if (lhs.pose.Translation().Distance(rhs.pose.Translation()).value() >
+      ERROR_MARGIN_TRANSLATION)
+    return false;
+  const frc::Rotation3d rot_diff = lhs.pose.Rotation() - rhs.pose.Rotation();
+  const double euler_diff = wrap_compare(rot_diff.X().value()) +
+                            wrap_compare(rot_diff.Y().value()) +
+                            wrap_compare(rot_diff.Z().value());
+  return euler_diff == 0;
+}
+}  // namespace frc
+
 constexpr int kimage_tag_width = 20;
 constexpr int kimage_tag_height = 20;
 constexpr std::array<int, 2> ktag_ids = {15, 31};
 constexpr int kimage_width = 20;
 constexpr int kimage_height = 20;
-constexpr int krotation = 1;
-
-auto main() -> int {
-  // Top left, Top right, Bottom right, Bottom left
-  const std::array<cv::Point2f, 4> image_points = {
-      cv::Point2f(kimage_width / 2.0 - kimage_tag_width / 2.0 + krotation,
-                  kimage_height / 2.0 + kimage_tag_height / 2.0),
-      cv::Point2f(kimage_width / 2.0 + kimage_tag_width / 2.0 - krotation,
-                  kimage_height / 2.0 + kimage_tag_height / 2.0),
-      cv::Point2f(kimage_width / 2.0 + kimage_tag_width / 2.0 - krotation,
-                  kimage_height / 2.0 - kimage_tag_height / 2.0),
-      cv::Point2f(kimage_width / 2.0 - kimage_tag_width / 2.0 + krotation,
-                  kimage_height / 2.0 - kimage_tag_height / 2.0)};
+const cv::Point2f tag_center =
+    cv::Point2f(kimage_width / 2.0, kimage_height / 2.0);
+const int dx = kimage_tag_width / 2.0;
+const int dy = kimage_tag_height / 2.0;
+// bottom left, bottom right, top right, top left
+const std::array<cv::Point2f, 4> image_points = {
+    cv::Point2f(tag_center.x - dx, tag_center.y + dy),
+    cv::Point2f(tag_center.x + dx, tag_center.y + dy),
+    cv::Point2f(tag_center.x + dx, tag_center.y - dy),
+    cv::Point2f(tag_center.x - dx, tag_center.y - dy)};
+
+TEST(SolverTest, Basic) {
+  for (const auto& point : image_points) {
+    std::cout << point << std::endl;
+  }
   localization::SquareSolver solver(camera::Camera::DUMMY_CAMERA);
 
   for (const int id : ktag_ids) {
     const localization::tag_detection_t fake_detection{.tag_id = id,
                                                        .corners = image_points};
     const std::vector<localization::tag_detection_t> fake_detections{
         fake_detection};
-    std::cout << id << ":\n"
-              << solver.EstimatePosition(fake_detections)[0] << std::endl;
-  }
 
-  return 0;
+    localization::position_estimate_t estimate =
+        solver.EstimatePosition(fake_detections)[0];
+    frc::Pose3d flipped_tag =
+        localization::kapriltag_layout.GetTagPose(id).value();
+    flipped_tag = frc::Pose3d(
+        flipped_tag.Translation(),
+        frc::Rotation3d(
+            flipped_tag.Rotation().X(), flipped_tag.Rotation().Y(),
+            flipped_tag.Rotation().Z() + units::radian_t{-std::numbers::pi / 2.0}));
+    localization::position_estimate_t expected{flipped_tag, 0, 0};
+    operator==(estimate, expected);
+    EXPECT_EQ(estimate, expected);
+  }
 }