diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..4c36b07 --- /dev/null +++ b/.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 + } + } + ] +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..4ace4f7 --- /dev/null +++ b/.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 + } + } + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 81a807f..54da154 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/constants/dummy_camera_extrinsics.json b/constants/dummy_camera_extrinsics.json index 0379d90..872284a 100644 --- a/constants/dummy_camera_extrinsics.json +++ b/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 } diff --git a/scripts/build.sh b/scripts/build.sh index b8a70fe..83f2ed8 100755 --- a/scripts/build.sh +++ b/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 \ No newline at end of file diff --git a/src/localization/position.h b/src/localization/position.h index beaf805..50dc5c8 100644 --- a/src/localization/position.h +++ b/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 { diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 0ab18df..d63d1b5 100644 --- a/src/test/CMakeLists.txt +++ b/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) diff --git a/src/test/solver_test.cc b/src/test/solver_test.cc index d87fb90..ec16de5 100644 --- a/src/test/solver_test.cc +++ b/src/test/solver_test.cc @@ -1,24 +1,66 @@ +#include +#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 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 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 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) { @@ -26,9 +68,18 @@ auto main() -> int { .corners = image_points}; const std::vector 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); + } }