From abe34ea0e653eb637632add4240ae9a0eb625a5e Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 5 Dec 2025 21:26:07 +0800 Subject: [PATCH 01/36] feat(solve_pnp): add OpenCV to ROS camera coordinate system conversion Implement transformation logic to convert camera coordinates from the OpenCV standard (Z forward, X right, Y down) to the ROS standard (X forward, Y left, Z up). - The PnP solver output (relative pose) is now consistently converted and reported in the ROS camera frame. - Add unit tests for the coordinate transformation function. - Integrate simulated dynamic transforms to test and verify the robustness of the coordinate frame transformations during runtime. --- src/component.cpp | 30 ++++- src/utility/math/conversion.hpp | 87 +++++++++++++ src/utility/math/solve_pnp.cpp | 46 +++---- src/utility/shared/context.hpp | 13 ++ test/CMakeLists.txt | 62 ++++++++++ test/convertion.cpp | 201 +++++++++++++++++++++++++++++++ test/solve_pnp.cpp | 18 +-- test/transform_communication.cpp | 137 +++++++++++++++++++++ 8 files changed, 562 insertions(+), 32 deletions(-) create mode 100644 src/utility/math/conversion.hpp create mode 100644 test/convertion.cpp create mode 100644 test/transform_communication.cpp diff --git a/src/component.cpp b/src/component.cpp index 7cea0b7a..3ce46fd7 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -21,6 +21,28 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { + using namespace rmcs_description; + if (rmcs_tf.ready()) [[likely]] { + auto camera_odom = + fast_tf::lookup_transform( + *rmcs_tf); + auto odom_to_muzzle = + fast_tf::lookup_transform( + *rmcs_tf); + + control_state.timestamp = Clock::now(); + + control_state.camera_to_odom_transform.posture = camera_odom.translation(); + control_state.camera_to_odom_transform.orientation = + Eigen::Quaterniond(camera_odom.rotation()); + + control_state.odom_to_muzzle_transform.posture = odom_to_muzzle.translation(); + control_state.odom_to_muzzle_transform.orientation = + Eigen::Quaterniond(odom_to_muzzle.rotation()); + + //... + } + recv_state(); send_state(); } @@ -33,6 +55,8 @@ class AutoAimComponent final : public rmcs_executor::Component { ControlClient::Send shm_send; ControlClient::Recv shm_recv; + ControlState control_state; + FramerateCounter framerate; private: @@ -46,6 +70,7 @@ class AutoAimComponent final : public rmcs_executor::Component { if (shm_recv.is_updated()) { auto timestamp = Stamp {}; + shm_recv.with_read([&](const auto& state) { timestamp = state.timestamp; }); if (shm_recv.is_updated()) { @@ -67,8 +92,9 @@ class AutoAimComponent final : public rmcs_executor::Component { return; } - shm_send.with_write([](ControlState& state) { - state.timestamp = Clock::now(); + shm_send.with_write([&](ControlState& state) { + state = control_state; + // ... }); } diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp new file mode 100644 index 00000000..fc6461f9 --- /dev/null +++ b/src/utility/math/conversion.hpp @@ -0,0 +1,87 @@ + +#pragma once +#include +#include + +#include "utility/math/linear.hpp" + +namespace rmcs::util { + +// 坐标变换公式： +// P_C_cv = R_cv * P_W + t_cv +// P_C_ros = T * (R_cv * P_W + t_cv) = (T * R_cv) * P_W + (T * t_cv) +// 因此： +// R_ros = T * R_cv +// t_ros = T * t_cv +// 其中 T 为从 OpenCV 光学系到 ROS camera_link 的基变换矩阵。 + +/// 生成从源坐标系到目标坐标系的转换矩阵 T，使得 v_target = T * v_source。 +/// 输入轴向量支持 linear.hpp 中的自定义三维向量或 Eigen::Vector3d，并保证为正交单位向量。 +template +inline auto make_basis_transform(const VecSrc& x_src, const VecSrc& y_src, const VecSrc& z_src, + const VecDst& x_dst, const VecDst& y_dst, const VecDst& z_dst) -> Eigen::Matrix3d { + + auto to_eigen = [](const auto& v) -> Eigen::Vector3d { + using T = std::decay_t; + if constexpr (rmcs::translation_object_trait) { + return { v.x(), v.y(), v.z() }; + } else { + return { v.x, v.y, v.z }; + } + }; + + const auto Xs = to_eigen(x_src); + const auto Ys = to_eigen(y_src); + const auto Zs = to_eigen(z_src); + const auto Xd = to_eigen(x_dst); + const auto Yd = to_eigen(y_dst); + const auto Zd = to_eigen(z_dst); + + [[maybe_unused]] auto is_unit = [](const Eigen::Vector3d& v) { + return std::abs(v.norm() - 1.0) < 1e-6; + }; + [[maybe_unused]] auto is_orth = [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) { + return std::abs(a.dot(b)) < 1e-6; + }; + + assert(is_unit(Xs) && is_unit(Ys) && is_unit(Zs) && "source must be unit length"); + assert(is_unit(Xd) && is_unit(Yd) && is_unit(Zd) && "target must be unit length"); + assert(is_orth(Xs, Ys) && is_orth(Ys, Zs) && is_orth(Zs, Xs) && "source must be orthogonal"); + assert(is_orth(Xd, Yd) && is_orth(Yd, Zd) && is_orth(Zd, Xd) && "target must be orthogonal"); + + Eigen::Matrix3d R_src; + R_src.col(0) = Xs; + R_src.col(1) = Ys; + R_src.col(2) = Zs; + + Eigen::Matrix3d R_dst; + R_dst.col(0) = Xd; + R_dst.col(1) = Yd; + R_dst.col(2) = Zd; + + // 将源坐标的分量映射到目标坐标分量 + return R_dst.transpose() * R_src; +} + +/// OpenCV 光学坐标系 (x:右, y:下, z:前) -> ROS camera_link (x:前, y:左, z:上) +inline auto make_cv_optical_to_ros_camera_link() -> Eigen::Matrix3d { + using V = Eigen::Vector3d; + const V x_cv { 1., 0., 0. }; + const V y_cv { 0., 1., 0. }; + const V z_cv { 0., 0., 1. }; + + const V x_ros { 0., 0., 1. }; // 前 + const V y_ros { -1., 0., 0. }; // 左 + const V z_ros { 0., -1., 0. }; // 上 + + return make_basis_transform(x_cv, y_cv, z_cv, x_ros, y_ros, z_ros); +} + +/// 将 solvePnP 的输出 (OpenCV 光学坐标系) 转换为 ROS camera_link 坐标系 +inline auto cv_optical_to_ros_camera_link(const Eigen::Matrix3d& R_cv, const Eigen::Vector3d& t_cv) + -> std::pair { + const auto T = make_cv_optical_to_ros_camera_link(); + return { T * R_cv, T * t_cv }; +} + +} diff --git a/src/utility/math/solve_pnp.cpp b/src/utility/math/solve_pnp.cpp index 1f0e898b..2719c381 100644 --- a/src/utility/math/solve_pnp.cpp +++ b/src/utility/math/solve_pnp.cpp @@ -1,4 +1,5 @@ #include "solve_pnp.hpp" +#include "utility/math/conversion.hpp" #include #include #include @@ -53,26 +54,29 @@ auto PnpSolution::solve() noexcept -> void { cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); - { - result.translation.x = tran_vec[0]; - result.translation.y = tran_vec[1]; - result.translation.z = tran_vec[2]; - } - { - auto rotation_opencv = cv::Mat {}; - cv::Rodrigues(rota_vec, rotation_opencv); + auto tran_vec_eigen_opencv = Eigen::Vector3d {}; + tran_vec_eigen_opencv.x() = tran_vec[0]; + tran_vec_eigen_opencv.y() = tran_vec[1]; + tran_vec_eigen_opencv.z() = tran_vec[2]; - auto rotation_eigen = Eigen::Matrix3d {}; - rotation_eigen << // Col Major - rotation_opencv.at(0, 0), // [0,0] - rotation_opencv.at(0, 1), // [0,1] - rotation_opencv.at(0, 2), // [0,2] - rotation_opencv.at(1, 0), // [1,0] - rotation_opencv.at(1, 1), // [1,1] - rotation_opencv.at(1, 2), // [1,2] - rotation_opencv.at(2, 0), // [2,0] - rotation_opencv.at(2, 1), // [2,1] - rotation_opencv.at(2, 2); // [2,2] - result.orientation = Eigen::Quaterniond { rotation_eigen }; - } + auto rotation_opencv = cv::Mat {}; + cv::Rodrigues(rota_vec, rotation_opencv); + + auto rotation_eigen_opencv = Eigen::Matrix3d {}; + rotation_eigen_opencv << // Col Major + rotation_opencv.at(0, 0), // [0,0] + rotation_opencv.at(0, 1), // [0,1] + rotation_opencv.at(0, 2), // [0,2] + rotation_opencv.at(1, 0), // [1,0] + rotation_opencv.at(1, 1), // [1,1] + rotation_opencv.at(1, 2), // [1,2] + rotation_opencv.at(2, 0), // [2,0] + rotation_opencv.at(2, 1), // [2,1] + rotation_opencv.at(2, 2); // [2,2] + + auto [rotation_eigen_ros, tran_vec_eigen_ros] = + rmcs::util::cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + + result.translation = tran_vec_eigen_ros; + result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; } diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 8b615b4b..3c35507d 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -16,6 +16,11 @@ enum class ShootMode { BUFF_LARGE, }; +struct Transform { + Direction3d posture {}; + Orientation orientation {}; +}; + struct AutoAimState { Stamp timestamp {}; @@ -34,6 +39,14 @@ struct ControlState { double bullet_speed {}; Orientation imu_state {}; + /*Note: + * 对应关系： + * odom<->fast_tf::OdomImu,muzzle_link<->fast_tf::MuzzleLink + * camera<->fast_tf::CameraLink + * */ + Transform odom_to_muzzle_transform {}; + Transform camera_to_odom_transform {}; + DeviceIds targets { DeviceIds::Full() }; }; static_assert(std::is_trivially_copyable_v); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 782f3577..33ef21a5 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -98,3 +98,65 @@ target_link_libraries( test_static_tf yaml-cpp::yaml-cpp ) + +# Conversion +ament_add_gtest( + test_conversion + ${TEST_DIR}/convertion.cpp +) + +# Transform Communication +ament_add_gtest( + test_transform_communication + ${TEST_DIR}/transform_communication.cpp +) + + +# Visualization +add_executable( + example_visualization + ${TEST_DIR}/visualization.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/visual/posture.cpp + ${RMCS_SRC_DIR}/utility/panic.cpp + ${RMCS_SRC_DIR}/utility/math/solve_armors.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp +) +target_link_libraries( + example_visualization + rclcpp::rclcpp + ${visualization_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} +) + +if(HIKCAMERA_AVAILABLE) + # Hikcamera + add_executable( + example_hikcamera + ${TEST_DIR}/hikcamera.cpp + ) + target_link_libraries( + example_hikcamera + ${OpenCV_LIBS} + ${hikcamera_LIBRARIES} + ) + + # RTP Streaming + add_executable( + example_streaming + ${TEST_DIR}/streaming.cpp + ${RMCS_SRC_DIR}/module/capturer/hikcamera.cpp + ${RMCS_SRC_DIR}/module/debug/visualization/stream_session.cpp + ${RMCS_SRC_DIR}/module/debug/visualization/stream_context.cpp + ${RMCS_SRC_DIR}/utility/image/image.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp + ) + target_link_libraries( + example_streaming + rclcpp::rclcpp + ${OpenCV_LIBS} + ${hikcamera_LIBRARIES} + ) +else() + message(STATUS "hikcamera 未检测到，跳过相关构建") +endif() diff --git a/test/convertion.cpp b/test/convertion.cpp new file mode 100644 index 00000000..c97fb797 --- /dev/null +++ b/test/convertion.cpp @@ -0,0 +1,201 @@ +#include + +#include "utility/math/conversion.hpp" +#include + +using namespace rmcs::util; + +namespace { + +template +void expect_matrix_near(const MatA& a, const MatB& b, double eps = 1e-9) { + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + EXPECT_NEAR(a(r, c), b(r, c), eps); + } + } +} + +} // namespace + +TEST(Conversion, BasisTransformIdentity) { + Eigen::Vector3d ex { 1, 0, 0 }; + Eigen::Vector3d ey { 0, 1, 0 }; + Eigen::Vector3d ez { 0, 0, 1 }; + + auto T = make_basis_transform(ex, ey, ez, ex, ey, ez); + + expect_matrix_near(T, Eigen::Matrix3d::Identity()); +} + +TEST(Conversion, CvOpticalToRosMatrix) { + const auto T = make_cv_optical_to_ros_camera_link(); + + Eigen::Matrix3d expected; + + // clang-format off + expected << + 0 , 0 , 1, + -1, 0 , 0, + 0 , -1, 0; + // clang-format on + + expect_matrix_near(T, expected); +} + +TEST(Conversion, CvToRosTransformRt) { + Eigen::Matrix3d R_cv = Eigen::Matrix3d::Identity(); + Eigen::Vector3d t_cv { 1., 2., 3. }; + + const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); + + Eigen::Matrix3d T; + + // clang-format off + T << + 0 , 0 , 1, + -1, 0 , 0, + 0 , -1, 0; + // clang-format on + + expect_matrix_near(R_ros, T); + + Eigen::Vector3d expected_t = T * t_cv; + EXPECT_DOUBLE_EQ(t_ros.x(), expected_t.x()); + EXPECT_DOUBLE_EQ(t_ros.y(), expected_t.y()); + EXPECT_DOUBLE_EQ(t_ros.z(), expected_t.z()); +} + +TEST(Conversion, BasisTransformRotate90Z) { + // 源系与目标系：目标系绕 Z 轴 +90 度 + Eigen::Vector3d ex { 1, 0, 0 }; + Eigen::Vector3d ey { 0, 1, 0 }; + Eigen::Vector3d ez { 0, 0, 1 }; + + Eigen::Vector3d ex_rot { 0, 1, 0 }; + Eigen::Vector3d ey_rot { -1, 0, 0 }; + Eigen::Vector3d ez_rot { 0, 0, 1 }; + + auto T = make_basis_transform(ex, ey, ez, ex_rot, ey_rot, ez_rot); + + Eigen::Matrix3d expected, actual; + // clang-format off + actual << + 0 , -1, 0, + 1 , 0, 0, + 0 , 0, 1; + // clang-format on + expected = actual.transpose(); + expect_matrix_near(T, expected); +} + +TEST(Conversion, CvToRosWithRotation) { + // 在 cv 系下绕 Z 轴 90 度，转换后应组合到 T 中 + const auto T = make_cv_optical_to_ros_camera_link(); + + Eigen::AngleAxisd aa_cv(M_PI / 2.0, Eigen::Vector3d::UnitZ()); + Eigen::Matrix3d R_cv = aa_cv.toRotationMatrix(); + Eigen::Vector3d t_cv { 0.5, -0.25, 2.0 }; + + const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); + + Eigen::Matrix3d expected_R = T * R_cv; + expect_matrix_near(R_ros, expected_R); + + Eigen::Vector3d expected_t = T * t_cv; + EXPECT_NEAR(t_ros.x(), expected_t.x(), 1e-12); + EXPECT_NEAR(t_ros.y(), expected_t.y(), 1e-12); + EXPECT_NEAR(t_ros.z(), expected_t.z(), 1e-12); +} + +TEST(Conversion, CvToRosWithNonIdentityBasis) { + // 非标准源/目标基的组合测试 + Eigen::Vector3d src_x { 0, 1, 0 }; // x 指向原来的 y + Eigen::Vector3d src_y { -1, 0, 0 }; // y 指向 -x + Eigen::Vector3d src_z { 0, 0, 1 }; + + Eigen::Vector3d dst_x { 0, 0, 1 }; // x -> 原来的 z + Eigen::Vector3d dst_y { 1, 0, 0 }; // y -> 原来的 x + Eigen::Vector3d dst_z { 0, 1, 0 }; // z -> 原来的 y + + auto T = make_basis_transform(src_x, src_y, src_z, dst_x, dst_y, dst_z); + + Eigen::Matrix3d expected; + // 手工计算：dst^T * src + expected << 0, 0, 1, 0, -1, 0, 1, 0, 0; + + expect_matrix_near(T, expected); +} + +TEST(Conversion, BasisVectorMapping) { + const auto T = make_cv_optical_to_ros_camera_link(); + + const Eigen::Vector3d ex_cv { 1, 0, 0 }; + const Eigen::Vector3d ey_cv { 0, 1, 0 }; + const Eigen::Vector3d ez_cv { 0, 0, 1 }; + + auto ex_ros = T * ex_cv; // 应为 (0,-1,0) + auto ey_ros = T * ey_cv; // 应为 (0,0,-1) + auto ez_ros = T * ez_cv; // 应为 (1,0,0) + + EXPECT_NEAR(ex_ros.x(), 0.0, 1e-12); + EXPECT_NEAR(ex_ros.y(), -1.0, 1e-12); + EXPECT_NEAR(ex_ros.z(), 0.0, 1e-12); + + EXPECT_NEAR(ey_ros.x(), 0.0, 1e-12); + EXPECT_NEAR(ey_ros.y(), 0.0, 1e-12); + EXPECT_NEAR(ey_ros.z(), -1.0, 1e-12); + + EXPECT_NEAR(ez_ros.x(), 1.0, 1e-12); + EXPECT_NEAR(ez_ros.y(), 0.0, 1e-12); + EXPECT_NEAR(ez_ros.z(), 0.0, 1e-12); +} + +TEST(Conversion, ArbitraryVectorTransform) { + Eigen::Vector3d v_cv { 1., 2., 3. }; + auto expected = Eigen::Vector3d { 3., -1., -2. }; + + auto [_, t_ros] = cv_optical_to_ros_camera_link(Eigen::Matrix3d::Identity(), v_cv); + EXPECT_NEAR(t_ros.x(), expected.x(), 1e-12); + EXPECT_NEAR(t_ros.y(), expected.y(), 1e-12); + EXPECT_NEAR(t_ros.z(), expected.z(), 1e-12); + + // TODO:manual calulate; +} + +TEST(Conversion, InverseTransform) { + const auto T = make_cv_optical_to_ros_camera_link(); + Eigen::Matrix3d T_inverse = T.inverse(); + + // 1. 验证逆矩阵是否等于转置 (验证纯旋转特性) + expect_matrix_near(T_inverse, T.transpose()); + + // 2. 验证 T 乘以 T^-1 是否等于单位矩阵 + Eigen::Matrix3d Identity = T * T_inverse; + expect_matrix_near(Identity, Eigen::Matrix3d::Identity()); +} + +TEST(Conversion, CompositeRotation) { + const auto T = make_cv_optical_to_ros_camera_link(); + + // R1: 绕 cv-x 轴 45° + Eigen::AngleAxisd aa_x(M_PI / 4.0, Eigen::Vector3d::UnitX()); + Eigen::Matrix3d R1 = aa_x.toRotationMatrix(); + + // R2: 绕 cv-y 轴 30° + Eigen::AngleAxisd aa_y(M_PI / 6.0, Eigen::Vector3d::UnitY()); + Eigen::Matrix3d R2 = aa_y.toRotationMatrix(); + + // 1. 源系 (CV) 复合旋转 + Eigen::Matrix3d R_cv_comp = R2 * R1; + + // 2. 期望的目标系复合结果 (T * (R2 * R1)) + Eigen::Matrix3d R_ros_expected = T * R_cv_comp; + + // 3. 实际计算 + auto [R_ros_actual, t_ros_acturl] = + cv_optical_to_ros_camera_link(R_cv_comp, Eigen::Vector3d::Zero()); + + // 验证复合结果是否正确 + expect_matrix_near(R_ros_actual, R_ros_expected); +} diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index d62da49b..083d1687 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -119,8 +119,8 @@ TEST_F(PnpSolverTest, BasicSmallArmor) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + EXPECT_GT(solution.result.translation.x, 0.0) // + << "Translation x should be positive (in front of camera)"; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // << "Quaternion should be normalized"; @@ -152,8 +152,8 @@ TEST_F(PnpSolverTest, BigArmor) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + EXPECT_GT(solution.result.translation.x, 0.0) // + << "Translation x should be positive (in front of camera)"; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // << "Quaternion should be normalized"; @@ -188,7 +188,7 @@ TEST_F(PnpSolverTest, DifferentDistances) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - const auto distance_error = std::abs(solution.result.translation.z - expected_distance); + const auto distance_error = std::abs(solution.result.translation.x - expected_distance); EXPECT_LT(distance_error, expected_distance * 0.5) // << "Distance error too large for expected distance " << expected_distance; @@ -218,8 +218,8 @@ TEST_F(PnpSolverTest, EdgeCaseSmallDetection) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + EXPECT_GT(solution.result.translation.x, 0.0) // + << "Translation x should be positive (in front of camera)"; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // << "Quaternion should be normalized"; @@ -245,8 +245,8 @@ TEST_F(PnpSolverTest, WithDistortion) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + EXPECT_GT(solution.result.translation.x, 0.0) // + << "Translation x should be positive (in front of camera)"; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // << "Quaternion should be normalized"; diff --git a/test/transform_communication.cpp b/test/transform_communication.cpp new file mode 100644 index 00000000..6e8f68cb --- /dev/null +++ b/test/transform_communication.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include + +#include "utility/shared/context.hpp" +#include "utility/shared/interprocess.hpp" +#include + +using Transform = rmcs::util::Transform; + +namespace { + +constexpr auto create_input_transform() -> Transform { + using rmcs::Direction3d; + using rmcs::Orientation; + return { + .posture = Direction3d { 1.0, 2.0, 3.0 }, + .orientation = Orientation { 0.0, 0.0, 0.0, 1.0 }, + }; +} + +// 非期望值，用于验证接收后被正确覆盖 +inline auto create_failed_input_transform() -> Transform { + using rmcs::Direction3d; + using rmcs::Orientation; + return { + .posture = Direction3d { 2.0, 3.0, 3.0 }, + .orientation = Orientation { Eigen::Quaterniond::Identity() }, + }; +} + +inline auto expect_transform_equal(const Transform& lhs, const Transform& rhs) -> void { + EXPECT_DOUBLE_EQ(lhs.posture.x, rhs.posture.x); + EXPECT_DOUBLE_EQ(lhs.posture.y, rhs.posture.y); + EXPECT_DOUBLE_EQ(lhs.posture.z, rhs.posture.z); + EXPECT_DOUBLE_EQ(lhs.orientation.x, rhs.orientation.x); + EXPECT_DOUBLE_EQ(lhs.orientation.y, rhs.orientation.y); + EXPECT_DOUBLE_EQ(lhs.orientation.z, rhs.orientation.z); + EXPECT_DOUBLE_EQ(lhs.orientation.w, rhs.orientation.w); +} + +} // namespace + +// 父子进程通信：确保写入的 Transform 能被完整读取 +TEST(TransformShm, SendRecvSequence) { + using Send = rmcs::shm::Client::Send; + using Recv = rmcs::shm::Client::Recv; + + using namespace std::chrono_literals; + + constexpr auto shm_name = "/rmcs_auto_aim_transform_shm_test"; + constexpr auto test_value = create_input_transform(); + constexpr auto max_attempts = 100; + constexpr auto poll_interval = 10ms; + constexpr auto init_delay = 50ms; + + // 父进程先创建共享内存对象 + auto send = Send {}; + ASSERT_TRUE(send.open(shm_name)); + ASSERT_TRUE(send.opened()); + + auto pid = fork(); + ASSERT_GE(pid, 0); + + if (pid == 0) { + // 子进程：接收数据 + auto recv = Recv {}; + ASSERT_TRUE(recv.open(shm_name)); + ASSERT_TRUE(recv.opened()); + + // 等待接收数据 + auto received_value = create_failed_input_transform(); + auto received = false; + + // 尝试接收数据，最多等待一段时间 + for (auto i = 0; i < max_attempts; ++i) { + if (recv.is_updated()) { + recv.with_read([&](const auto& data) { received_value = data; }); + received = true; + break; + } + std::this_thread::sleep_for(poll_interval); + } + + ASSERT_TRUE(received && "failed to receive"); + + expect_transform_equal(received_value, test_value); + + std::exit(0); + } else { + // 父进程：发送数据 + // 等待一下子进程准备好 + std::this_thread::sleep_for(init_delay); + + // 发送数据 + send.with_write([&](auto& data) { data = test_value; }); + + // 等待子进程完成 + auto status = int { 0 }; + waitpid(pid, &status, 0); + ASSERT_TRUE(WIFEXITED(status)); + ASSERT_EQ(WEXITSTATUS(status), 0); + } +} + +// 单进程读写：验证版本控制与快照读取的正确性，模拟 component 中动态变换的发布/消费 +TEST(TransformShm, SnapshotAndUpdate) { + using Client = rmcs::shm::Client; + Client::Send send; + Client::Recv recv; + + constexpr auto shm_name = "/rmcs_auto_aim_snapshot_test"; + + ASSERT_TRUE(send.open(shm_name)); + ASSERT_TRUE(recv.open(shm_name)); + + const auto first = create_input_transform(); + const auto second = create_failed_input_transform(); + + // 初次写入，接收端应检测到更新 + send.send(first); + EXPECT_TRUE(recv.is_updated()); + + Transform snapshot {}; + recv.with_read([&](const auto& data) { snapshot = data; }); + expect_transform_equal(snapshot, first); + EXPECT_FALSE(recv.is_updated()); // 读取后版本同步 + + // 再写入一次不同数据，接收端应再次检测到更新 + send.with_write([&](auto& data) { data = second; }); + EXPECT_TRUE(recv.is_updated()); + + recv.recv(snapshot); // 使用 recv 接口读取一次 + expect_transform_equal(snapshot, second); + EXPECT_FALSE(recv.is_updated()); +} From 9414c0e50da82934b3b6a1479edb439a2a502cb5 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Mon, 8 Dec 2025 04:29:32 +0800 Subject: [PATCH 02/36] feat(aim): implement inter-system communication and PnP pipeline visualization Successfully established communication link between the control system and the aiming module. This enables data exchange for target tracking and control. The full PnP (Perspective-n-Point) solving pipeline has been integrated and is running. The resulting pose (translation and rotation) is now visualized in the debugging interface to assist in development. Note: The output coordinate frame for the PnP solution is currently uncalibrated and requires further refinement/tuning to align with the world coordinate system. --- config/config.yaml | 3 +- src/component.cpp | 7 -- src/kernel/control_system.cpp | 9 ++ src/kernel/pose_estimator.cpp | 60 +++++++++++- src/kernel/pose_estimator.hpp | 11 +++ .../pose_estimator/pnp_solution.cpp} | 66 ++++++------- src/module/pose_estimator/pnp_solution.hpp | 39 ++++++++ src/runtime.cpp | 12 ++- src/utility/math/solve_pnp.hpp | 92 ++++++++++++++----- src/utility/rclcpp/visual/armor.cpp | 64 +++++++++---- src/utility/rclcpp/visual/armor.hpp | 4 + src/utility/robot/id.hpp | 5 +- src/utility/shared/context.hpp | 3 +- test/CMakeLists.txt | 8 +- test/solve_pnp.cpp | 1 + 15 files changed, 283 insertions(+), 101 deletions(-) rename src/{utility/math/solve_pnp.cpp => module/pose_estimator/pnp_solution.cpp} (57%) create mode 100644 src/module/pose_estimator/pnp_solution.hpp diff --git a/config/config.yaml b/config/config.yaml index ec76c174..6bdfa671 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,6 @@ use_visualization: true -use_painted_image: true +use_painted_image: false +open_solved_pnp_visualization: true capturer: show_loss_framerate: false diff --git a/src/component.cpp b/src/component.cpp index 3ce46fd7..5c6d8f93 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -26,9 +26,6 @@ class AutoAimComponent final : public rmcs_executor::Component { auto camera_odom = fast_tf::lookup_transform( *rmcs_tf); - auto odom_to_muzzle = - fast_tf::lookup_transform( - *rmcs_tf); control_state.timestamp = Clock::now(); @@ -36,10 +33,6 @@ class AutoAimComponent final : public rmcs_executor::Component { control_state.camera_to_odom_transform.orientation = Eigen::Quaterniond(camera_odom.rotation()); - control_state.odom_to_muzzle_transform.posture = odom_to_muzzle.translation(); - control_state.odom_to_muzzle_transform.orientation = - Eigen::Quaterniond(odom_to_muzzle.rotation()); - //... } diff --git a/src/kernel/control_system.cpp b/src/kernel/control_system.cpp index 3b5b4aae..050613b8 100644 --- a/src/kernel/control_system.cpp +++ b/src/kernel/control_system.cpp @@ -10,6 +10,15 @@ struct ControlSystem::Impl { ControlState control_state {}; + Impl() noexcept { + if (!shm_recv.open(util::shared_control_state_name)) { + util::panic("Failed to open shared control state"); + } + if (!shm_send.open(util::shared_autoaim_state_name)) { + util::panic("Failed to open shared autoaim state"); + } + } + /// Send template F> diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index cf4558d3..dc0e6a2b 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -1,4 +1,5 @@ #include "pose_estimator.hpp" + #include "kernel/transform_tree.hpp" #include "utility/logging/printer.hpp" #include "utility/math/solve_pnp.hpp" @@ -25,9 +26,9 @@ struct PoseEstimator::Impl { }; Config config; - Printer log { "PoseEstimator" }; + std::vector pnp_solutions {}; - PnpSolution pnp_solution; + Printer log { "PoseEstimator" }; auto initialize(const YAML::Node& yaml) noexcept -> std::expected try { auto result = config.serialize(yaml); @@ -49,7 +50,52 @@ struct PoseEstimator::Impl { return std::unexpected { e.what() }; } - auto transform() { } + auto solve_pnp(std::optional> const& armors) noexcept -> void { + if (!armors.has_value()) return; + + auto _armors = (*armors); + pnp_solutions.reserve(_armors.size()); + pnp_solutions.resize(_armors.size()); + + auto color = [](ArmorColor const& color) -> CampColor { + if (color == ArmorColor::BLUE) return CampColor::BLUE; + if (color == ArmorColor::RED) return CampColor::RED; + return CampColor::UNKNOWN; + }; + + auto shape = [](ArmorShape shape) -> std::array { + if (shape == ArmorShape::SMALL) { + return rmcs::kLargeArmorShape; + } else { + return rmcs::kSmallArmorShape; + } + }; + + auto const _camera_matrix = reshape_array(config.camera_matrix); + auto const _distort_coeff = reshape_array(config.distort_coeff); + + std::ranges::for_each(std::views::zip(_armors, pnp_solutions), + [&color, &shape, &_camera_matrix, &_distort_coeff](auto&& pair) { + auto const& [armor, pnp_solution] = pair; + + PnpSolution& solution = const_cast(pnp_solution); + + solution.input.armor_shape = shape(armor.shape); + solution.input.genre = armor.genre; + solution.input.color = color(armor.color); + std::ranges::copy(armor.corners(), solution.input.armor_detection.begin()); + solution.input.camera_matrix = _camera_matrix; + solution.input.distort_coeff = _distort_coeff; + + solution.solve(); + }); + } + + auto visualize(RclcppNode& visual_node) -> void { + for (auto& solution : pnp_solutions) { + solution.visualize(visual_node); + } + } }; auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept @@ -57,6 +103,14 @@ auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept return pimpl->initialize(yaml); } +void PoseEstimator::solve_pnp(std::optional> const& armors) const noexcept { + return pimpl->solve_pnp(armors); +} + +auto PoseEstimator::visualize(RclcppNode& visual_node) -> void { + return pimpl->visualize(visual_node); +} + PoseEstimator::PoseEstimator() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 2c30f633..2cf0a741 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -1,6 +1,10 @@ #pragma once + +#include "module/pose_estimator/pnp_solution.hpp" #include "utility/math/linear.hpp" #include "utility/pimpl.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" #include #include @@ -10,8 +14,15 @@ class PoseEstimator { RMCS_PIMPL_DEFINITION(PoseEstimator) public: + using PnpSolution = util::PnpSolution; + using RclcppNode = util::RclcppNode; + auto initialize(const YAML::Node&) noexcept -> std::expected; + auto solve_pnp(std::optional> const&) const noexcept -> void; + + auto visualize(RclcppNode& visual_node) -> void; + auto update_imu_link(const Orientation&) noexcept -> void; }; diff --git a/src/utility/math/solve_pnp.cpp b/src/module/pose_estimator/pnp_solution.cpp similarity index 57% rename from src/utility/math/solve_pnp.cpp rename to src/module/pose_estimator/pnp_solution.cpp index 2719c381..fc190963 100644 --- a/src/utility/math/solve_pnp.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -1,44 +1,11 @@ -#include "solve_pnp.hpp" -#include "utility/math/conversion.hpp" -#include -#include -#include -#include +#include "pnp_solution.hpp" -using namespace rmcs::util; - -template -static auto cast_opencv_matrix(std::array& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } - - return cv::Mat { 1, cols, mat_type, source.data() }; -} - -template -static auto cast_opencv_matrix(std::array, rows>& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } +#include - return cv::Mat { rows, cols, mat_type, source[0].data() }; -} +#include "utility/math/conversion.hpp" +#include "utility/math/solve_pnp.hpp" +using namespace rmcs::util; auto PnpSolution::solve() noexcept -> void { const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); @@ -63,7 +30,7 @@ auto PnpSolution::solve() noexcept -> void { cv::Rodrigues(rota_vec, rotation_opencv); auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Col Major + rotation_eigen_opencv << // Row Major rotation_opencv.at(0, 0), // [0,0] rotation_opencv.at(0, 1), // [0,1] rotation_opencv.at(0, 2), // [0,2] @@ -75,8 +42,27 @@ auto PnpSolution::solve() noexcept -> void { rotation_opencv.at(2, 2); // [2,2] auto [rotation_eigen_ros, tran_vec_eigen_ros] = - rmcs::util::cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + result.genre = input.genre; + result.color = input.color; result.translation = tran_vec_eigen_ros; result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; } + +auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { + using namespace visual; + if (!visualized_armor) { + auto const config = Armor::Config { + + .rclcpp = visual_node, + .device = result.genre, + .camp = result.color, + .id = "solved_pnp_armor", + .tf = "camera_link", + }; + visualized_armor = std::make_unique(config); + } + visualized_armor->impl_move(result.translation, result.orientation); + visualized_armor->update(); +} diff --git a/src/module/pose_estimator/pnp_solution.hpp b/src/module/pose_estimator/pnp_solution.hpp new file mode 100644 index 00000000..05055bff --- /dev/null +++ b/src/module/pose_estimator/pnp_solution.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "utility/math/linear.hpp" +#include "utility/math/point.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/rclcpp/visual/armor.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::util { + +struct PnpSolution { + struct Input { + // Row Major + std::array, 3> camera_matrix; + std::array distort_coeff; + std::array armor_shape; + std::array armor_detection; + DeviceId genre; + CampColor color; + } input; + struct Result { + Translation translation; + Orientation orientation; + DeviceId genre; + CampColor color; + } result; + + std::unique_ptr visualized_armor; + + PnpSolution() noexcept = default; + + auto solve() noexcept -> void; + + auto visualize(RclcppNode&) noexcept -> void; +}; +} diff --git a/src/runtime.cpp b/src/runtime.cpp index a1f53770..1fba77d2 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,6 +6,7 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" +#include "utility/logging/printer.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" @@ -23,6 +24,7 @@ auto main() -> int { std::signal(SIGINT, [](int) { util::set_running(false); }); auto rclcpp_node = util::RclcppNode { "AutoAim" }; + rclcpp_node.set_pub_topic_prefix("/rmcs/auto_aim/"); auto handle_result = [&](auto runtime_name, const auto& result) { if (!result.has_value()) { @@ -46,9 +48,10 @@ auto main() -> int { /// Configure /// - auto configuration = util::configuration(); - auto use_visualization = configuration["use_visualization"].as(); - auto use_painted_image = configuration["use_painted_image"].as(); + auto configuration = util::configuration(); + auto use_visualization = configuration["use_visualization"].as(); + auto use_painted_image = configuration["use_painted_image"].as(); + auto open_solved_pnp_visualization = configuration["open_solved_pnp_visualization"].as(); // CAPTURER { @@ -80,6 +83,8 @@ auto main() -> int { handle_result("visualization", result); } + rmcs::Printer printer { "rmcs_auto_aim" }; + for (;;) { if (!util::get_running()) [[unlikely]] break; @@ -118,3 +123,4 @@ auto main() -> int { rclcpp_node.shutdown(); } + diff --git a/src/utility/math/solve_pnp.hpp b/src/utility/math/solve_pnp.hpp index 806b098a..f7ffd7f8 100644 --- a/src/utility/math/solve_pnp.hpp +++ b/src/utility/math/solve_pnp.hpp @@ -1,26 +1,72 @@ #pragma once -#include "utility/math/linear.hpp" -#include "utility/math/point.hpp" -#include - -namespace rmcs::util { - -struct PnpSolution { - struct Input { - // Row Major - std::array, 3> camera_matrix; - std::array distort_coeff; - std::array armor_shape; - std::array armor_detection; - } input; - struct Result { - Translation translation; - Orientation orientation; - } result; - - PnpSolution() noexcept = default; - - auto solve() noexcept -> void; -}; +#include +#include +#include + +template +static auto cast_opencv_matrix(std::array& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { 1, cols, mat_type, source.data() }; +} + +template +static auto cast_opencv_matrix(std::array, rows>& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { rows, cols, mat_type, source[0].data() }; +} + +template +concept ConvertibleTo = std::is_convertible_v; +/* @Note: Row-Major */ +template + requires ConvertibleTo +static auto reshape_array(std::array const& input_array) + -> std::array, rows> { + static_assert(N == rows * cols, "input_array的元素总数N必须等于rows*cols"); + using ResultArray = std::array, rows>; + + ResultArray result_array; + for (std::size_t i = 0; i < N; ++i) { + std::size_t row_idx = i / cols; + std::size_t col_idx = i % cols; + + result_array[row_idx][col_idx] = input_array[i]; + } + return result_array; +} + +/* @Note: Row-Major */ +template + requires ConvertibleTo +static auto reshape_array(std::array const& input_array) + -> std::array { + using ResultArray = std::array; + + ResultArray result_array; + std::transform(input_array.begin(), input_array.end(), result_array.begin(), + [](const input_type& val) { return static_cast(val); }); + + return result_array; } diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index f60be4bd..dbb08d7f 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -1,8 +1,6 @@ #include "armor.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/node.details.hpp" -#include -#include using namespace rmcs::util::visual; @@ -11,47 +9,69 @@ using Marker = visualization_msgs::msg::Marker; struct Armor::Impl { static inline rclcpp::Clock rclcpp_clock { RCL_SYSTEM_TIME }; + std::unique_ptr config; + Marker marker; std::shared_ptr> rclcpp_pub; - explicit Impl(const Config& config) { - auto& rclcpp = config.rclcpp; - auto& details = config.rclcpp.details; + explicit Impl(const Config& config) + : config(std::make_unique(config)) { + initialize(); + } - if (!prefix::check_naming(config.id) || !prefix::check_naming(config.tf)) { - util::panic( - std::format("Not a valid naming for armor id or tf: {}", prefix::naming_standard)); + static auto create_rclcpp_publisher(Config const& config) + -> std::shared_ptr> { + + const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.id }; + + if (!config.rclcpp.details) { + util::panic("Rclcpp node details are required in config to create publisher."); } - const auto topic_name { rclcpp.get_pub_topic_prefix() + config.id }; - rclcpp_pub = details->make_pub(topic_name, qos::debug); + return config.rclcpp.details->make_pub(topic_name, qos::debug); + } - marker.header.frame_id = config.tf; + auto initialize() -> void { + if (!prefix::check_naming(config->id) || !prefix::check_naming(config->tf)) { + util::panic( + std::format("Not a valid naming for armor id or tf: {}", prefix::naming_standard)); + } - marker.ns = config.id; - marker.id = 0; - marker.type = Marker::CUBE; - marker.action = Marker::ADD; + marker.header.frame_id = config->tf; + marker.ns = config->id; + marker.id = 0; + marker.type = Marker::CUBE; + marker.action = Marker::ADD; // ref: "https://www.robomaster.com/zh-CN/products/components/detail/149" - /* */ if (DeviceIds::kSmallArmorDevices().contains(config.device)) { + /* */ if (DeviceIds::kSmallArmorDevices().contains(config->device)) { marker.scale.x = 0.003, marker.scale.y = 0.140, marker.scale.z = 0.125; - } else if (DeviceIds::kLargeArmorDevices().contains(config.device)) { + } else if (DeviceIds::kLargeArmorDevices().contains(config->device)) { marker.scale.x = 0.003, marker.scale.y = 0.235, marker.scale.z = 0.127; } else { util::panic("Wrong device id for a visualized armor"); }; - /* */ if (config.camp == CampColor::RED) { + /* */ if (config->camp == CampColor::RED) { marker.color.r = 1., marker.color.g = 0., marker.color.b = 0., marker.color.a = 1.; - } else if (config.camp == CampColor::BLUE) { + } else if (config->camp == CampColor::BLUE) { marker.color.r = 0., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; } else { util::panic("Please specify a valid armor color"); } } + auto set_external_rclcpp_publisher( + std::shared_ptr> const& _rclcpp_pub) -> void { + rclcpp_pub = _rclcpp_pub; + } + auto update() noexcept -> void { + if (!rclcpp_pub) { + const auto topic_name { config->rclcpp.get_pub_topic_prefix() + config->id }; + rclcpp_pub = config->rclcpp.details->make_pub(topic_name, qos::debug); + } + marker.header.stamp = rclcpp_clock.now(); rclcpp_pub->publish(marker); } @@ -71,4 +91,10 @@ auto Armor::impl_move(const Translation& t, const Orientation& q) noexcept -> vo Armor::Armor(const Config& config) noexcept : pimpl { std::make_unique(config) } { } +Armor::Armor( + Config const& config, std::shared_ptr> const& publisher) noexcept + : pimpl { std::make_unique(config) } { + pimpl->set_external_rclcpp_publisher(publisher); +} + Armor::~Armor() noexcept = default; diff --git a/src/utility/rclcpp/visual/armor.hpp b/src/utility/rclcpp/visual/armor.hpp index 5b6bf36a..c1f3c4da 100644 --- a/src/utility/rclcpp/visual/armor.hpp +++ b/src/utility/rclcpp/visual/armor.hpp @@ -3,6 +3,8 @@ #include "utility/rclcpp/visual/movable.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" +#include +#include namespace rmcs::util::visual { @@ -19,6 +21,8 @@ struct Armor : public Movable { }; explicit Armor(const Config&) noexcept; + explicit Armor(Config const&, + std::shared_ptr> const&) noexcept; ~Armor() noexcept; Armor(const Armor&) = delete; diff --git a/src/utility/robot/id.hpp b/src/utility/robot/id.hpp index 5ca91c67..9452c450 100644 --- a/src/utility/robot/id.hpp +++ b/src/utility/robot/id.hpp @@ -86,8 +86,9 @@ struct DeviceIds { static constexpr auto None() { return DeviceIds {}; } static constexpr auto Full() { return DeviceIds { (1 << 11) - 1 }; } - constexpr DeviceIds() = default; - constexpr DeviceIds(const DeviceIds&) = default; + constexpr DeviceIds() = default; + constexpr DeviceIds(const DeviceIds&) = default; + constexpr DeviceIds& operator=(const DeviceIds&) = default; constexpr explicit DeviceIds(uint16_t data) noexcept : data { data } { }; diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 3c35507d..3152ad47 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -41,10 +41,9 @@ struct ControlState { /*Note: * 对应关系： - * odom<->fast_tf::OdomImu,muzzle_link<->fast_tf::MuzzleLink + * odom<->fast_tf::OdomImu, * camera<->fast_tf::CameraLink * */ - Transform odom_to_muzzle_transform {}; Transform camera_to_odom_transform {}; DeviceIds targets { DeviceIds::Full() }; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 33ef21a5..7ea2d056 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -81,11 +81,17 @@ ament_add_gtest( ament_add_gtest( test_solve_pnp ${TEST_DIR}/solve_pnp.cpp - ${RMCS_SRC_DIR}/utility/math/solve_pnp.cpp + ${RMCS_SRC_DIR}/module/pose_estimator/pnp_solution.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp + ${RMCS_SRC_DIR}/utility/panic.cpp ) target_link_libraries( test_solve_pnp ${OpenCV_LIBRARIES} + rclcpp::rclcpp + ${visualization_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} ) # Static TF diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index 083d1687..a570fc9b 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -1,4 +1,5 @@ #include "utility/math/solve_pnp.hpp" +#include "module/pose_estimator/pnp_solution.hpp" #include "utility/math/linear.hpp" #include "utility/math/point.hpp" From 495f90ad8ca579efd8e9e1ca5bea6a4edf9962d3 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Mon, 8 Dec 2025 22:50:50 +0800 Subject: [PATCH 03/36] feat(capturer): implement local video stream acquisition Successfully implemented the functionality to acquire and process video streams from local storage or device. --- config/config.yaml | 4 +- src/module/capturer/local_video.cpp | 63 ++++++++++++++++++++++++++--- src/module/capturer/local_video.hpp | 5 ++- src/runtime.cpp | 5 +-- 4 files changed, 66 insertions(+), 11 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 6bdfa671..877b2c44 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -6,7 +6,7 @@ capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - source: "hikcamera" + source: "local_video" hikcamera: # int timeout_ms: 500 @@ -22,7 +22,7 @@ capturer: trigger_mode: false fixed_framerate: true local_video: - location: "" + location: "/workspaces/RMCS/solve_pnp.mp4" loop_play: true identifier: diff --git a/src/module/capturer/local_video.cpp b/src/module/capturer/local_video.cpp index 64bc1bf7..fc39b3c4 100644 --- a/src/module/capturer/local_video.cpp +++ b/src/module/capturer/local_video.cpp @@ -1,16 +1,69 @@ #include "local_video.hpp" +#include "utility/image/image.details.hpp" + +#include +#include + using namespace rmcs::cap; +using rmcs::Image; -struct LocalVideo::Impl { }; +struct LocalVideo::Impl { + cv::VideoCapture cap; + ConfigDetail config; + + auto release() noexcept { + if (cap.isOpened()) cap.release(); + } +}; auto LocalVideo::configure(const ConfigDetail& config) noexcept - -> std::expected { } + -> std::expected { + pimpl->config = config; + + if (pimpl->config.location.empty()) { + return std::unexpected { "Video location is empty" }; + } + if (!std::filesystem::exists(pimpl->config.location)) { + return std::unexpected { "Video file not found: " + pimpl->config.location }; + } + return {}; +} + +auto LocalVideo::connect() noexcept -> std::expected { + pimpl->release(); + + if (!pimpl->cap.open(pimpl->config.location)) { + return std::unexpected { "Failed to open video: " + pimpl->config.location }; + } + return {}; +} + +auto LocalVideo::connected() const noexcept -> bool { return pimpl->cap.isOpened(); } + +auto LocalVideo::wait_image() -> std::expected, std::string> { + if (!connected()) { + return std::unexpected { "Video is not opened" }; + } -auto LocalVideo::connect() noexcept -> std::expected { } + cv::Mat frame; + if (!pimpl->cap.read(frame) || frame.empty()) { + if (pimpl->config.loop_play) { + pimpl->cap.set(cv::CAP_PROP_POS_FRAMES, 0); + if (pimpl->cap.read(frame) && !frame.empty()) { + } else { + return std::unexpected { "Failed to read frame even after seeking to beginning" }; + } + } + return std::unexpected { "Failed to read frame" }; + } -auto LocalVideo::connected() const noexcept -> bool { } + auto image = std::make_unique(); + image->details().set_mat(std::move(frame)); + image->set_timestamp(Image::Clock::now()); + return image; +} -auto LocalVideo::wait_image() -> std::expected, std::string> { } +auto LocalVideo::disconnect() noexcept -> void { pimpl->release(); } LocalVideo::LocalVideo() noexcept : pimpl { std::make_unique() } { } diff --git a/src/module/capturer/local_video.hpp b/src/module/capturer/local_video.hpp index 69ae1818..570368a7 100644 --- a/src/module/capturer/local_video.hpp +++ b/src/module/capturer/local_video.hpp @@ -12,12 +12,15 @@ class LocalVideo { public: struct ConfigDetail { std::string location; + bool loop_play; }; struct Config : ConfigDetail, util::Serializable { constexpr static std::tuple metas { &Config::location, "location", + &Config::loop_play, + "loop_play", }; }; @@ -25,7 +28,7 @@ class LocalVideo { auto connect() noexcept -> std::expected; - auto disconnect() noexcept { } + auto disconnect() noexcept -> void; auto connected() const noexcept -> bool; diff --git a/src/runtime.cpp b/src/runtime.cpp index 1fba77d2..375d224c 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -63,9 +63,9 @@ auto main() -> int { { auto config = configuration["identifier"]; - const auto path = std::filesystem::path { util::Parameters::share_location() } + const auto model_location = std::filesystem::path { util::Parameters::share_location() } / std::filesystem::path { config["model_location"].as() }; - config["model_location"] = path.string(); + config["model_location"] = model_location.string(); auto result = identifier.initialize(config); handle_result("identifier", result); @@ -123,4 +123,3 @@ auto main() -> int { rclcpp_node.shutdown(); } - From 9018123033238a9cafe63e302b74f46c67111ba2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 10 Dec 2025 22:17:51 +0800 Subject: [PATCH 04/36] fix(video, pnp): resolve LocalVideo bug and correct PnP point ordering - Corrected a critical bug in the `LocalVideo` module . - Fixed an issue in the `SolvePnp` function where the ordering of 2D image points and their corresponding 3D object points was mismatched, leading to incorrect pose estimation results. The PnP output is now using the correct point correspondence. Further dedicated unit tests for the `SolvePnp` function need to be added in the next step to ensure robustness. --- config/config.yaml | 8 +- src/kernel/pose_estimator.cpp | 4 +- src/module/capturer/local_video.cpp | 145 +++++++++++++++------ src/module/capturer/local_video.hpp | 28 ++-- src/module/pose_estimator/pnp_solution.cpp | 5 +- src/utility/robot/armor.hpp | 24 +++- 6 files changed, 153 insertions(+), 61 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 877b2c44..49459db4 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,5 @@ use_visualization: true -use_painted_image: false +use_painted_image: true open_solved_pnp_visualization: true capturer: @@ -22,8 +22,14 @@ capturer: trigger_mode: false fixed_framerate: true local_video: + # 替换为你具体的路径 location: "/workspaces/RMCS/solve_pnp.mp4" + # double 帧率 + frame_rate: 80 + # bool 是否循环播放 loop_play: true + # bool 是否允许跳帧以满足实时性 + allow_skipping: false identifier: binarization_threshold: 0.5 diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index dc0e6a2b..afe9ba0a 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -65,9 +65,9 @@ struct PoseEstimator::Impl { auto shape = [](ArmorShape shape) -> std::array { if (shape == ArmorShape::SMALL) { - return rmcs::kLargeArmorShape; + return rmcs::kLargeArmorShapeOpenCV; } else { - return rmcs::kSmallArmorShape; + return rmcs::kSmallArmorShapeOpenCV; } }; diff --git a/src/module/capturer/local_video.cpp b/src/module/capturer/local_video.cpp index fc39b3c4..efd640fb 100644 --- a/src/module/capturer/local_video.cpp +++ b/src/module/capturer/local_video.cpp @@ -1,69 +1,130 @@ #include "local_video.hpp" -#include "utility/image/image.details.hpp" #include +#include + #include +#include "utility/image/image.details.hpp" + using namespace rmcs::cap; -using rmcs::Image; struct LocalVideo::Impl { - cv::VideoCapture cap; - ConfigDetail config; + Config config; - auto release() noexcept { - if (cap.isOpened()) cap.release(); - } -}; + using Clock = std::chrono::steady_clock; -auto LocalVideo::configure(const ConfigDetail& config) noexcept - -> std::expected { - pimpl->config = config; + std::optional capturer; - if (pimpl->config.location.empty()) { - return std::unexpected { "Video location is empty" }; - } - if (!std::filesystem::exists(pimpl->config.location)) { - return std::unexpected { "Video file not found: " + pimpl->config.location }; - } - return {}; -} + std::chrono::nanoseconds interval_duration { 0 }; + Clock::time_point last_read_time { Clock::now() }; + + auto set_framerate_interval(double hz) noexcept -> void { + if (hz > 0) { + interval_duration = + std::chrono::nanoseconds(static_cast(std::round(1.0 / hz * 1e9))); + } else { + interval_duration = std::chrono::nanoseconds { 0 }; + } + }; + + auto configure(Config const& _config) -> std::expected { + if (_config.location.empty() || !std::filesystem::exists(_config.location)) { + return std::unexpected { "Local video is not found or location is empty" }; + } + + config = _config; + + try { + capturer.emplace(config.location); + } catch (std::exception const& e) { + return std::unexpected { "Failed to construct VideoCapture: " + std::string(e.what()) }; + } catch (...) { + return std::unexpected { "Failed to construct VideoCapture due to an unknown error." }; + } + + double source_fps = capturer->get(cv::CAP_PROP_FPS); + double target_fps = source_fps > 0 ? source_fps : 30.0; + + if (config.frame_rate > 0) { + target_fps = config.frame_rate; + } -auto LocalVideo::connect() noexcept -> std::expected { - pimpl->release(); + set_framerate_interval(target_fps); - if (!pimpl->cap.open(pimpl->config.location)) { - return std::unexpected { "Failed to open video: " + pimpl->config.location }; + last_read_time = Clock::now(); + + return {}; } - return {}; -} -auto LocalVideo::connected() const noexcept -> bool { return pimpl->cap.isOpened(); } + auto connect() -> std::expected { return configure(config); } + + auto connected() const noexcept -> bool { return capturer.has_value() && capturer->isOpened(); } + + auto disconnect() noexcept -> std::expected { + if (capturer.has_value()) { + capturer.reset(); + } + interval_duration = std::chrono::nanoseconds { 0 }; -auto LocalVideo::wait_image() -> std::expected, std::string> { - if (!connected()) { - return std::unexpected { "Video is not opened" }; + return {}; } - cv::Mat frame; - if (!pimpl->cap.read(frame) || frame.empty()) { - if (pimpl->config.loop_play) { - pimpl->cap.set(cv::CAP_PROP_POS_FRAMES, 0); - if (pimpl->cap.read(frame) && !frame.empty()) { + auto wait_image() noexcept -> std::expected, std::string> { + if (!capturer.has_value() || !capturer->isOpened()) { + return std::unexpected { "Video stream is not opened." }; + } + + const auto time_before_read = Clock::now(); + const auto next_read_time_expected = last_read_time + interval_duration; + auto wait_duration = next_read_time_expected - time_before_read; + + if (wait_duration.count() > 0) { + std::this_thread::sleep_for(wait_duration); + last_read_time = next_read_time_expected; + } else { + last_read_time = config.allow_skipping ? Clock::now() : next_read_time_expected; + } + + auto frame = cv::Mat {}; + auto image = std::make_unique(); + if (!capturer->read(frame)) { + if (config.loop_play) { + if (capturer->set(cv::CAP_PROP_POS_FRAMES, 0) && capturer->read(frame)) { + last_read_time = Clock::now(); + } else { + return std::unexpected { "End of file reached and failed to " + "loop/reset." }; + } } else { - return std::unexpected { "Failed to read frame even after seeking to beginning" }; + return std::unexpected { "End of file reached." }; } } - return std::unexpected { "Failed to read frame" }; - } - auto image = std::make_unique(); - image->details().set_mat(std::move(frame)); - image->set_timestamp(Image::Clock::now()); - return image; + if (frame.empty()) { + return std::unexpected { "Read frame is empty, possibly due to IO error." }; + } + image->details().set_mat(frame); + image->set_timestamp(last_read_time); + + return image; + }; +}; + +auto LocalVideo::configure(Config const& config) -> std::expected { + return pimpl->configure(config); } +auto LocalVideo::wait_image() noexcept -> std::expected, std::string> { + return pimpl->wait_image(); +} + +auto LocalVideo::connect() noexcept -> std::expected { return pimpl->connect(); } -auto LocalVideo::disconnect() noexcept -> void { pimpl->release(); } +auto LocalVideo::connected() const noexcept -> bool { return pimpl->connected(); } + +auto LocalVideo::disconnect() noexcept -> std::expected { + return pimpl->disconnect(); +} LocalVideo::LocalVideo() noexcept : pimpl { std::make_unique() } { } diff --git a/src/module/capturer/local_video.hpp b/src/module/capturer/local_video.hpp index 570368a7..a0bdaba7 100644 --- a/src/module/capturer/local_video.hpp +++ b/src/module/capturer/local_video.hpp @@ -6,33 +6,39 @@ namespace rmcs::cap { -class LocalVideo { +struct LocalVideo { RMCS_PIMPL_DEFINITION(LocalVideo) -public: +private: struct ConfigDetail { std::string location; + double frame_rate; bool loop_play; + bool allow_skipping; }; +public: struct Config : ConfigDetail, util::Serializable { constexpr static std::tuple metas { - &Config::location, - "location", - &Config::loop_play, - "loop_play", + &ConfigDetail::location, + "location", // 视频文件路径 + &ConfigDetail::frame_rate, + "frame_rate", // 帧率 + &ConfigDetail::loop_play, + "loop_play", // 循环播放 + &ConfigDetail::allow_skipping, + "allow_skipping" // 允许跳帧以保证实时性 }; }; - auto configure(const ConfigDetail&) noexcept -> std::expected; + auto configure(Config const&) -> std::expected; auto connect() noexcept -> std::expected; - auto disconnect() noexcept -> void; - auto connected() const noexcept -> bool; - auto wait_image() -> std::expected, std::string>; -}; + auto disconnect() noexcept -> std::expected; + auto wait_image() noexcept -> std::expected, std::string>; +}; } diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/module/pose_estimator/pnp_solution.cpp index fc190963..7e1e5282 100644 --- a/src/module/pose_estimator/pnp_solution.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -30,7 +30,7 @@ auto PnpSolution::solve() noexcept -> void { cv::Rodrigues(rota_vec, rotation_opencv); auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Row Major + rotation_eigen_opencv << // Major rotation_opencv.at(0, 0), // [0,0] rotation_opencv.at(0, 1), // [0,1] rotation_opencv.at(0, 2), // [0,2] @@ -43,11 +43,12 @@ auto PnpSolution::solve() noexcept -> void { auto [rotation_eigen_ros, tran_vec_eigen_ros] = cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); result.genre = input.genre; result.color = input.color; result.translation = tran_vec_eigen_ros; - result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; + result.orientation = orientation_ros; } auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 7d7aaed7..3e3110b4 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -48,13 +48,31 @@ struct Armor3D { }; struct Armor { }; using Armors = std::vector; -constexpr std::array kLargeArmorShape { +constexpr double kLightBarHeight = 0.056; +constexpr double kLargeArmorWidth = 0.23; +constexpr double kSmallArmorWidth = 0.135; + +constexpr std::array kLargeArmorShapeOpenCV { + Point3d { -0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left + Point3d { 0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right + Point3d { 0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { -0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left +}; + +constexpr std::array kSmallArmorShapeOpenCV { + Point3d { -0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left + Point3d { 0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right + Point3d { 0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { -0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left +}; + +constexpr std::array kLargeArmorShapeRos { Point3d { 0.0, 0.115, 0.028 }, // Top-left - Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, -0.115, -0.028 }, // Bottom-right + Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, 0.115, -0.028 } // Bottom-left }; -constexpr std::array kSmallArmorShape { +constexpr std::array kSmallArmorShapeRos { Point3d { 0.0, 0.0675, 0.028 }, // Top-left Point3d { 0.0, -0.0675, 0.028 }, // Top-right Point3d { 0.0, -0.0675, -0.028 }, // Bottom-right From 9c228c6bbcd16166e1b64a74d356da02544b7d2a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 10 Dec 2025 22:31:12 +0800 Subject: [PATCH 05/36] test(pnp): SolvePnp verified and accuracy meets specification The PnP (Perspective-n-Point) solver has passed comprehensive unit testing for various target poses and input data sets. The measured positional and rotational errors are consistently maintained within the required 5% tolerance, confirming the numerical stability and accuracy of the current PnP implementation. --- test/solve_pnp.cpp | 119 ++++++++++++++++++--------------------------- 1 file changed, 46 insertions(+), 73 deletions(-) diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index a570fc9b..c2651e37 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -2,6 +2,7 @@ #include "module/pose_estimator/pnp_solution.hpp" #include "utility/math/linear.hpp" #include "utility/math/point.hpp" +#include "utility/robot/armor.hpp" #include #include @@ -26,42 +27,13 @@ PnpSolution::Input create_test_input(double focal_length = 800.0, double cx = 32 return input; } -// 辅助函数：创建小装甲板的 3D 点 +// 辅助函数：使用 OpenCV 坐标系定义的装甲板 3D 点（z 轴向外） std::array create_small_armor_shape() { - constexpr double ARMOR_WIDTH = 0.135; // 135mm - constexpr double ARMOR_HEIGHT = 0.056; // 56mm - - const auto eigen_points = std::array { - Point3d { 0.0, ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, // 右上 - Point3d { 0.0, -ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, // 右下 - Point3d { 0.0, -ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, // 左下 - Point3d { 0.0, ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 } // 左上 - }; - - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point3d(eigen_points[i]); - } - return points; + return rmcs::kSmallArmorShapeOpenCV; } -// 辅助函数：创建大装甲板的 3D 点 std::array create_big_armor_shape() { - constexpr double BIG_ARMOR_WIDTH = 0.230; // 230mm - constexpr double ARMOR_HEIGHT = 0.056; // 56mm - - const auto eigen_points = std::array { - Point3d { 0.0, BIG_ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, -BIG_ARMOR_WIDTH / 2.0, ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, -BIG_ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, - Point3d { 0.0, BIG_ARMOR_WIDTH / 2.0, -ARMOR_HEIGHT / 2.0 }, - }; - - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point3d(eigen_points[i]); - } - return points; + return rmcs::kLargeArmorShapeOpenCV; } // 辅助函数：从 Eigen::Vector2d 创建 Point2d 数组 @@ -107,11 +79,12 @@ TEST_F(PnpSolverTest, BasicSmallArmor) { solution.input.armor_shape = create_small_armor_shape(); // 使用 Eigen::Vector2d 创建 2D 检测点 + // 像素点按 TL,BR,TR,BL 顺序 const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + Vector2d { 290.0, 220.0 }, // TL + Vector2d { 350.0, 260.0 }, // BR + Vector2d { 350.0, 220.0 }, // TR + Vector2d { 290.0, 260.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -141,10 +114,10 @@ TEST_F(PnpSolverTest, BigArmor) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { - Vector2d { 380.0, 200.0 }, - Vector2d { 260.0, 200.0 }, - Vector2d { 260.0, 280.0 }, - Vector2d { 380.0, 280.0 }, + Vector2d { 260.0, 200.0 }, // TL + Vector2d { 380.0, 280.0 }, // BR + Vector2d { 380.0, 200.0 }, // TR + Vector2d { 260.0, 280.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -177,10 +150,10 @@ TEST_F(PnpSolverTest, DifferentDistances) { const auto center = Vector2d { 320.0, 240.0 }; const auto eigen_detection = std::array { - center + Vector2d { pixel_size / 2, -pixel_size * 0.2 }, - center + Vector2d { -pixel_size / 2, -pixel_size * 0.2 }, - center + Vector2d { -pixel_size / 2, pixel_size * 0.2 }, - center + Vector2d { pixel_size / 2, pixel_size * 0.2 }, + center + Vector2d { -pixel_size / 2, -pixel_size * 0.2 }, // TL + center + Vector2d { pixel_size / 2, pixel_size * 0.2 }, // BR + center + Vector2d { pixel_size / 2, -pixel_size * 0.2 }, // TR + center + Vector2d { -pixel_size / 2, pixel_size * 0.2 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -191,7 +164,7 @@ TEST_F(PnpSolverTest, DifferentDistances) { // 验证结果 const auto distance_error = std::abs(solution.result.translation.x - expected_distance); - EXPECT_LT(distance_error, expected_distance * 0.5) // + EXPECT_LT(distance_error, expected_distance * 0.05) //误差5% << "Distance error too large for expected distance " << expected_distance; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // @@ -207,10 +180,10 @@ TEST_F(PnpSolverTest, EdgeCaseSmallDetection) { // 使用 Eigen::Vector2d 创建 2D 检测点（小检测框，远距离） const auto eigen_detection = std::array { - Vector2d { 322.0, 238.0 }, - Vector2d { 318.0, 238.0 }, - Vector2d { 318.0, 242.0 }, - Vector2d { 322.0, 242.0 }, + Vector2d { 318.0, 238.0 }, // TL + Vector2d { 322.0, 242.0 }, // BR + Vector2d { 322.0, 238.0 }, // TR + Vector2d { 318.0, 242.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -234,10 +207,10 @@ TEST_F(PnpSolverTest, WithDistortion) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + Vector2d { 290.0, 220.0 }, // TL + Vector2d { 350.0, 260.0 }, // BR + Vector2d { 350.0, 220.0 }, // TR + Vector2d { 290.0, 260.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -261,10 +234,10 @@ TEST_F(PnpSolverTest, QuaternionValidity) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + Vector2d { 290.0, 220.0 }, // TL + Vector2d { 350.0, 260.0 }, // BR + Vector2d { 350.0, 220.0 }, // TR + Vector2d { 290.0, 260.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -293,10 +266,10 @@ TEST_F(PnpSolverTest, TranslationValidity) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + Vector2d { 290.0, 220.0 }, // TL + Vector2d { 350.0, 260.0 }, // BR + Vector2d { 350.0, 220.0 }, // TR + Vector2d { 290.0, 260.0 }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -311,7 +284,7 @@ TEST_F(PnpSolverTest, TranslationValidity) { EXPECT_TRUE(std::isfinite(t.y)) << "Translation y should be finite"; EXPECT_TRUE(std::isfinite(t.z)) << "Translation z should be finite"; - EXPECT_GT(t.z, 0.0) << "Translation z should be positive"; + EXPECT_GT(t.x, 0.0) << "Translation x should be positive"; } TEST_F(PnpSolverTest, DifferentFocalLengths) { @@ -328,10 +301,10 @@ TEST_F(PnpSolverTest, DifferentFocalLengths) { const auto offset = Vector2d { 30.0, 20.0 }; const auto eigen_detection = std::array { - center + Vector2d { offset.x() * scale, -offset.y() * scale }, - center + Vector2d { -offset.x() * scale, -offset.y() * scale }, - center + Vector2d { -offset.x() * scale, offset.y() * scale }, - center + Vector2d { offset.x() * scale, offset.y() * scale }, + center + Vector2d { -offset.x() * scale, -offset.y() * scale }, // TL + center + Vector2d { offset.x() * scale, offset.y() * scale }, // BR + center + Vector2d { offset.x() * scale, -offset.y() * scale }, // TR + center + Vector2d { -offset.x() * scale, offset.y() * scale }, // BL }; solution.input.armor_detection = create_armor_detection(eigen_detection); @@ -340,8 +313,8 @@ TEST_F(PnpSolverTest, DifferentFocalLengths) { EXPECT_NO_THROW(solution.solve()); // 验证结果 - EXPECT_GT(solution.result.translation.z, 0.0) // - << "Translation z should be positive (in front of camera)"; + EXPECT_GT(solution.result.translation.x, 0.0) // + << "Translation x should be positive (in front of camera)"; EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // << "Quaternion should be normalized"; @@ -357,10 +330,10 @@ TEST_F(PnpSolverTest, Consistency) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 350.0, 260.0 }, + Vector2d { 290.0, 220.0 }, // TL + Vector2d { 350.0, 260.0 }, // BR + Vector2d { 350.0, 220.0 }, // TR + Vector2d { 290.0, 260.0 }, // BL }; solution1.input.armor_detection = create_armor_detection(eigen_detection); @@ -394,8 +367,8 @@ TEST_F(PnpSolverTest, Performance) { // 使用 Eigen::Vector2d 创建 2D 检测点 const auto eigen_detection = std::array { Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 220.0 }, Vector2d { 290.0, 260.0 }, + Vector2d { 290.0, 220.0 }, Vector2d { 350.0, 260.0 }, }; From 179c9d0137534c4f21e6b5f43e4e909cc134c0e8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 10 Dec 2025 23:25:34 +0800 Subject: [PATCH 06/36] docs(utility): update documentation for utility functions --- doc/utility.md | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 doc/utility.md diff --git a/doc/utility.md b/doc/utility.md new file mode 100644 index 00000000..d30b1140 --- /dev/null +++ b/doc/utility.md @@ -0,0 +1,98 @@ +# Utility 工具库文档 + +本文档描述了 `utility` 工具库中各个模块的功能和用途。 + +## 设计理念 + +本工具库中的许多组件是为了**隐藏第三方库的头文件**而设计的包装器。第三方库（如 ROS2 的 rclcpp、OpenCV、Eigen、PCL 等）的头文件会增加编译成本，影响编译体验。通过使用包装器提供有限确定的接口，可以： + +- **减少编译时间**：避免在头文件中包含大型第三方库头文件 +- **降低编译依赖**：使用 PIMPL 模式将实现细节隐藏在 `.cpp` 文件中 +- **保持接口稳定**：提供简洁、稳定的接口，减少对第三方库内部变化的依赖 +- **改善编译体验**：只暴露必要的接口，减少不必要的类型暴露 + +因此，建议优先使用这些包装器而不是直接包含原始第三方库头文件。 + +## 核心工具 + +### 线性代数 +- **[linear.hpp](../src/utility/math/linear.hpp)**: 提供 `Translation` 和 `Orientation` 结构体，用于表示三维空间中的平移和旋转。支持从符合特定 trait 的对象进行转换，并提供 `copy_to` 方法将数据复制到目标对象。 + +### 错误处理 +- **[panic.hpp](../src/utility/panic.hpp)** / **[panic.cpp](../src/utility/panic.cpp)**: 提供 `panic` 函数，用于程序异常终止。会输出详细的错误信息，包括消息、文件位置、函数名、行号、线程 ID、时间戳和堆栈跟踪。 + +### 设计模式 +- **[pimpl.hpp](../src/utility/pimpl.hpp)**: 提供 PIMPL（Pointer to Implementation）模式的宏定义 `RMCS_PIMPL_DEFINITION`，用于隐藏实现细节。 +- **[details.hpp](../src/utility/details.hpp)**: 提供 Details 模式的宏定义 `RMCS_DETAILS_DEFINITION`，用于分离接口和实现细节。 + +### 序列化 +- **[serializable.hpp](../src/utility/serializable.hpp)**: 提供序列化框架，支持从 YAML 节点或 rclcpp 节点读取参数。使用 `Serializable` 结构体和 `MemberMeta` 来定义可序列化的成员，支持类型安全的参数读取。 + +### 次数限制 +- **[times_limit.hpp](../src/utility/times_limit.hpp)**: 提供 `TimesLimit` 类，用于限制某个操作的执行次数。支持启用/禁用、重置计数等功能。 + +### 图像处理（OpenCV 包装器） +- **[image.hpp](../src/utility/image/image.hpp)** / **[image.cpp](../src/utility/image/image.cpp)**: 提供 `Image` 类，封装图像数据和时间戳。使用 PIMPL 模式隐藏 OpenCV 头文件，避免在头文件中暴露 `cv::Mat` 等 OpenCV 类型。 +- **[image.details.hpp](../src/utility/image/image.details.hpp)**: 定义 `Image::Details` 结构体，包含 OpenCV 的 `cv::Mat` 和相关的访问方法。此文件包含 OpenCV 头文件，应仅在实现文件中使用。 +- **[image/painter.hpp](../src/utility/image/image/painter.hpp)**: 图像绘制工具（具体实现需查看源码）。 + +### ROS2 节点（rclcpp 包装器） +- **[node.hpp](../src/utility/node.hpp)**: 提供 `Node` 类，继承自 `rclcpp::Node`，扩展了日志功能，支持使用 `std::format` 进行格式化输出。提供 `info`、`warn`、`error` 等方法。**注意**：此文件直接继承 rclcpp::Node，如需完全隐藏 rclcpp 头文件，请使用 `rclcpp/node.hpp` 中的 `RclcppNode`。 + +### 鸭子类型 +- **[duck_type.hpp](../src/utility/duck_type.hpp)**: 提供 `duck_array` 模板类，实现鸭子类型检查。支持在编译时验证类型是否满足特定接口要求，并提供类型安全的元素访问。 + +## 子模块 + +### 协程 (coroutine) +- **[coroutine/channel.hpp](../src/utility/coroutine/channel.hpp)**: 提供协程通道 `Channel`，用于协程之间的数据传递。支持发送和接收操作，当通道为空时自动挂起等待。 +- **[coroutine/common.hpp](../src/utility/coroutine/common.hpp)**: 提供协程任务 `task` 模板，用于管理协程的生命周期。支持立即启动、延迟销毁，并能处理异常。 +- **[coroutine/context.hpp](../src/utility/coroutine/context.hpp)**: 定义协程上下文类（当前为空实现）。 + +### 数学工具 (math) +- **[math/sigmoid.hpp](../src/utility/math/sigmoid.hpp)**: 提供 sigmoid 函数实现，用于数值稳定计算。 +- **[math/solve_armors.hpp](../src/utility/math/solve_armors.hpp)**: 提供装甲板正解和逆解算法。`ArmorsForwardSolution` 用于根据机器人位姿计算四个装甲板的位置和姿态。 +- **[math/solve_armors.cpp](../src/utility/math/solve_armors.cpp)**: 正解算法的实现。 +- **[math/solve_pnp.hpp](../src/utility/math/solve_pnp.cpp)**: 提供将相机内外参的转换函数供pnp解算使用。 + +### ROS2 扩展 (rclcpp 包装器) +- **[rclcpp/node.hpp](../src/utility/rclcpp/node.hpp)**: 提供 `RclcppNode` 类，封装 ROS2 节点的基本功能，包括日志、发布主题前缀管理等。**使用 PIMPL 模式完全隐藏 rclcpp 头文件**，推荐在需要避免编译依赖时使用。 +- **[rclcpp/node.cpp](../src/utility/rclcpp/node.cpp)**: `RclcppNode` 的实现，包含所有 rclcpp 相关的头文件。 +- **[rclcpp/node.details.hpp](../src/utility/rclcpp/node.details.hpp)**: `RclcppNode::Details` 的定义，包含 rclcpp 类型，应仅在实现文件中使用。 +- **[rclcpp/parameters.hpp](../src/utility/rclcpp/parameters.hpp)**: 提供参数接口 `IParams` 和参数管理类 `Parameters`，用于统一参数访问接口。**隐藏 rclcpp 参数访问的细节**。 +- **[rclcpp/parameters.cpp](../src/utility/rclcpp/parameters.cpp)**: `Parameters` 的实现，包含 rclcpp 头文件。 +- **[rclcpp/rclcpp_param.hpp](../src/utility/rclcpp/rclcpp_param.hpp)**: 提供 `make_params` 函数，用于从 rclcpp 节点创建参数接口实现。**此文件包含 rclcpp 头文件**，应在实现文件中使用。 +- **[rclcpp/configuration.hpp](../src/utility/rclcpp/configuration.hpp)**: 提供 `configuration` 函数，用于从 YAML 文件加载配置。**此文件包含 YAML-CPP 头文件**。 +- **[rclcpp/visual/armor.hpp](../src/utility/rclcpp/visual/armor.hpp)** / **[rclcpp/visual/armor.cpp](../src/utility/rclcpp/visual/armor.cpp)**: ROS2 可视化消息相关（装甲板可视化），封装 ROS2 可视化消息类型。 + +### 机器人相关 (robot) +- **[robot/armor.hpp](../src/utility/robot/armor.hpp)**: 定义装甲板相关类型，包括 `ArmorColor`（颜色枚举）、`ArmorShape`（形状枚举）、`ArmorType`（类型）、`LightStrip`（灯条）和 `Armor` 结构体。 +- **[robot/color.hpp](../src/utility/robot/color.hpp)**: 定义 `CampColor` 枚举，表示阵营颜色（未知、红色、蓝色）。 +- **[robot/id.hpp](../src/utility/robot/id.hpp)**: 定义 `DeviceId` 枚举和 `DeviceIds` 类，用于标识不同类型的机器人设备（英雄、工程、步兵、哨兵等）。提供设备 ID 的位操作和查询功能。 + +### 日志 (logging) +- **[logging/printer.hpp](../src/utility/logging/printer.hpp)**: 提供 `Printer` 类，用于日志输出。支持不同级别的日志（INFO、WARN、ERROR），使用 `std::format` 进行格式化。 +- **[logging/printer.cpp](../src/utility/logging/printer.cpp)**: `Printer` 的实现。 + +### 共享内存 (shared) +- **[shared/context.hpp](../src/utility/shared/context.hpp)**: 定义共享上下文结构体，包含时间戳和字节数组，用于进程间数据共享。 +- **[shared/interprocess.hpp](../src/utility/shared/interprocess.hpp)**: 提供进程间通信的共享内存客户端。`Client` 模板类包含 `Send` 和 `Recv` 两个类，分别用于发送和接收数据。使用版本号机制确保数据一致性。 + +### 单例模式 (singleton) +- **[singleton/running.hpp](../src/utility/singleton/running.hpp)**: 提供运行状态管理的全局函数 `get_running` 和 `set_running`。 +- **[singleton/running.cpp](../src/utility/singleton/running.cpp)**: 运行状态管理的实现。 + +### 线程工具 (thread) +- **[thread/spsc_queue.hpp](../src/utility/thread/spsc_queue.hpp)**: 提供单生产者单消费者（SPSC）无锁队列的别名定义，基于 Boost.Lockfree 库。 +- **[thread/workers.hpp](../src/utility/thread/workers.hpp)**: 提供 `WorkersContext` 类，用于管理工作线程池。支持提交任务并返回 future，任务必须是 noexcept 可调用的。 +- **[thread/workers.cpp](../src/utility/thread/workers.cpp)**: `WorkersContext` 的实现。 + +### 模型 (model) +- **[model/armor_detection.hpp](../src/utility/model/armor_detection.hpp)**: 定义 `ArmorDetection` 结构体，用于表示检测到的装甲板信息。包含角点坐标、置信度、颜色信息和角色信息。支持从原始数据直接反序列化，并提供边界框计算和角点缩放功能。**注意**：此文件包含 OpenCV 头文件（`opencv2/core/types.hpp`），如需完全隐藏 OpenCV 依赖，建议使用包装器模式。 + + +## 一点个性 + +### ASCII 艺术 +- **[acsii_art.hpp](../src/utility/acsii_art.hpp)**: 提供 ASCII 艺术横幅，用于程序启动时显示 RMCS 标识。 + From 9a9baf8d6774568d00e508e0b7364d952b425570 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 12 Dec 2025 02:51:48 +0800 Subject: [PATCH 07/36] fix(deps, pnp): resolve cv2eigen dependency and correct identifier point ordering - Corrected the dependency issue related to conversion functions, ensuring seamless data exchange between OpenCV and Eigen types. - Rectified the misplacement/misordering issue with the four identifier points used in the pose estimation pipeline. This ensures that the 2D image points and their corresponding 3D object points are correctly matched. Note: The core PnP (Perspective-n-Point) pose estimation accuracy issue is still under investigation and has not yet been resolved in this commit. --- CMakeLists.txt | 2 +- config/config.yaml | 14 +-- package.xml | 3 +- src/kernel/pose_estimator.cpp | 4 +- src/module/identifier/model.cpp | 1 + src/module/pose_estimator/pnp_solution.cpp | 110 +++++++++++++++------ src/module/pose_estimator/pnp_solution.hpp | 1 + src/utility/math/solve_pnp.hpp | 1 + src/utility/model/armor_detection.hpp | 5 +- src/utility/rclcpp/visual/armor.cpp | 55 +++++++++-- src/utility/rclcpp/visual/armor.hpp | 4 +- src/utility/robot/armor.hpp | 7 +- 12 files changed, 149 insertions(+), 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 50225817..1fba3c12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 23) set(CMAKE_BUILD_TYPE "Release") add_compile_options(-Wall -Wextra -Wpedantic -O3) - +add_definitions(-DOPENCV_DISABLE_EIGEN_TENSOR_SUPPORT) ## 依赖查找 find_package(yaml-cpp REQUIRED) find_package(OpenCV 4.5 REQUIRED) diff --git a/config/config.yaml b/config/config.yaml index 49459db4..0cac0678 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -11,9 +11,9 @@ capturer: # int timeout_ms: 500 # float - exposure_us: 3000.0 + exposure_us: 2000.0 # float - framerate: 120 + framerate: 60 # float gain: 16.9807 @@ -23,9 +23,9 @@ capturer: fixed_framerate: true local_video: # 替换为你具体的路径 - location: "/workspaces/RMCS/solve_pnp.mp4" + location: "/workspaces/alliance/test_videos/solve_pnp_v2.mp4" # double 帧率 - frame_rate: 80 + frame_rate: 60 # bool 是否循环播放 loop_play: true # bool 是否允许跳帧以满足实时性 @@ -48,8 +48,8 @@ identifier: nms_threshold: 0.3 pose_estimator: - camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1] - distort_coeff: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0] + camera_matrix: [1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03,5.645821718351237e+02 , 0, 0, 1] + distort_coeff: [-0.064232403853946, -0.087667493884102, 0,0, 0.792381808294582] transforms: - parent: "imu_link" @@ -67,6 +67,6 @@ pose_estimator: visualization: framerate: 60 - monitor_host: "127.0.0.1" + monitor_host: "192.168.2.154" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/package.xml b/package.xml index f7e5b742..61f86b26 100644 --- a/package.xml +++ b/package.xml @@ -14,8 +14,9 @@ rmcs_executor + hikcamera ament_cmake - \ No newline at end of file + diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index afe9ba0a..1b163192 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -65,9 +65,9 @@ struct PoseEstimator::Impl { auto shape = [](ArmorShape shape) -> std::array { if (shape == ArmorShape::SMALL) { - return rmcs::kLargeArmorShapeOpenCV; - } else { return rmcs::kSmallArmorShapeOpenCV; + } else { + return rmcs::kLargeArmorShapeOpenCV; } }; diff --git a/src/module/identifier/model.cpp b/src/module/identifier/model.cpp index dff83fcc..285ecddc 100644 --- a/src/module/identifier/model.cpp +++ b/src/module/identifier/model.cpp @@ -234,6 +234,7 @@ struct OpenVinoNet::Impl { for (std::size_t row = 0; row < rows; row++) { auto line = armor_type {}; + //(0,1) 顶左，(2,3) 底左，(4,5) 底右，(6,7) 顶右 line.unsafe_from(std::span { data + row * cols, cols }); line.confidence = util::sigmoid(line.confidence); diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/module/pose_estimator/pnp_solution.cpp index 7e1e5282..58c09bfd 100644 --- a/src/module/pose_estimator/pnp_solution.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -1,14 +1,29 @@ #include "pnp_solution.hpp" -#include - #include "utility/math/conversion.hpp" #include "utility/math/solve_pnp.hpp" +#include +#include +#include +#include +#include using namespace rmcs::util; auto PnpSolution::solve() noexcept -> void { - const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); - const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); + // const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); + // const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); + + auto camera_matrix = Eigen::Matrix {}; + camera_matrix << 1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03, + 5.645821718351237e+02, 0, 0, 1; + + auto distort_coeff = Eigen::Matrix {}; + distort_coeff << -0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582; + + auto camera_matrix_ = cv::Mat {}; + auto distort_coeff_ = cv::Mat {}; + cv::eigen2cv(camera_matrix, camera_matrix_); + cv::eigen2cv(distort_coeff, distort_coeff_); const auto armor_shape = std::ranges::to(input.armor_shape | std::views::transform([](const Point3d& point) { return point.make(); })); @@ -16,39 +31,62 @@ auto PnpSolution::solve() noexcept -> void { const auto armor_detection = std::ranges::to(input.armor_detection | std::views::transform([](const Point2d& point) { return point.make(); })); + // fmt::print("K:\n"); + // for (const auto& row : input.camera_matrix) { + // fmt::print(" [{:f}, {:f}, {:f}]\n", row[0], row[1], row[2]); + // } + // fmt::print("D: [{:f}, {:f}, {:f}, {:f}, {:f}]\n", input.distort_coeff[0], + // input.distort_coeff[1], input.distort_coeff[2], input.distort_coeff[3], + // input.distort_coeff[4]); + // + // std::cout << "armor_shape:\n"; + // for (const auto& p : armor_shape) { + // std::cout << " (" << p.x << ", " << p.y << ", " << p.z << ")\n"; + // } + // std::cout << "armor_detection:\n"; + // for (const auto& p : armor_detection) { + // std::cout << " (" << p.x << ", " << p.y << ")\n"; + // } + auto rota_vec = cv::Vec3d {}; auto tran_vec = cv::Vec3d {}; - cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, + cv::solvePnP(armor_shape, armor_detection, camera_matrix_, distort_coeff_, rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); auto tran_vec_eigen_opencv = Eigen::Vector3d {}; - tran_vec_eigen_opencv.x() = tran_vec[0]; - tran_vec_eigen_opencv.y() = tran_vec[1]; - tran_vec_eigen_opencv.z() = tran_vec[2]; + cv::cv2eigen(tran_vec, tran_vec_eigen_opencv); + // tran_vec_eigen_opencv.x() = tran_vec[0]; + // tran_vec_eigen_opencv.y() = tran_vec[1]; + // tran_vec_eigen_opencv.z() = tran_vec[2]; auto rotation_opencv = cv::Mat {}; cv::Rodrigues(rota_vec, rotation_opencv); auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Major - rotation_opencv.at(0, 0), // [0,0] - rotation_opencv.at(0, 1), // [0,1] - rotation_opencv.at(0, 2), // [0,2] - rotation_opencv.at(1, 0), // [1,0] - rotation_opencv.at(1, 1), // [1,1] - rotation_opencv.at(1, 2), // [1,2] - rotation_opencv.at(2, 0), // [2,0] - rotation_opencv.at(2, 1), // [2,1] - rotation_opencv.at(2, 2); // [2,2] - - auto [rotation_eigen_ros, tran_vec_eigen_ros] = - cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); - auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); - - result.genre = input.genre; - result.color = input.color; - result.translation = tran_vec_eigen_ros; - result.orientation = orientation_ros; + cv::cv2eigen(rotation_opencv, rotation_eigen_opencv); + // rotation_eigen_opencv << // Major + // rotation_opencv.at(0, 0), // [0,0] + // rotation_opencv.at(0, 1), // [0,1] + // rotation_opencv.at(0, 2), // [0,2] + // rotation_opencv.at(1, 0), // [1,0] + // rotation_opencv.at(1, 1), // [1,1] + // rotation_opencv.at(1, 2), // [1,2] + // rotation_opencv.at(2, 0), // [2,0] + // rotation_opencv.at(2, 1), // [2,1] + // rotation_opencv.at(2, 2); // [2,2] + // + // rotation_eigen_opencv.transpose(); + // + // auto [rotation_eigen_ros, tran_vec_eigen_ros] = + // cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + // auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); + + result.genre = input.genre; + result.color = input.color; + // result.translation = tran_vec_eigen_ros; + // result.orientation = orientation_ros; + result.translation = tran_vec_eigen_opencv; + result.orientation = Eigen::Quaterniond(rotation_eigen_opencv).normalized(); } auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { @@ -62,8 +100,22 @@ auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { .id = "solved_pnp_armor", .tf = "camera_link", }; - visualized_armor = std::make_unique(config); + visualized_armor = std::make_unique(config); + auto const target_config = Armor::Config { + + .rclcpp = visual_node, + .device = result.genre, + .camp = result.color, + .id = "target_pnp_armor", + .tf = "camera_link", + }; + target_armor = std::make_unique(target_config); } - visualized_armor->impl_move(result.translation, result.orientation); + visualized_armor->move(result.translation, result.orientation); visualized_armor->update(); + + auto tran = Translation { 0, 0, 1.75 }; + auto orientation = Orientation { 0, 0.7071, 0, 0.7071 }; + target_armor->move(tran, orientation); + target_armor->update(); } diff --git a/src/module/pose_estimator/pnp_solution.hpp b/src/module/pose_estimator/pnp_solution.hpp index 05055bff..90d2eff1 100644 --- a/src/module/pose_estimator/pnp_solution.hpp +++ b/src/module/pose_estimator/pnp_solution.hpp @@ -29,6 +29,7 @@ struct PnpSolution { } result; std::unique_ptr visualized_armor; + std::unique_ptr target_armor; PnpSolution() noexcept = default; diff --git a/src/utility/math/solve_pnp.hpp b/src/utility/math/solve_pnp.hpp index f7ffd7f8..257626d6 100644 --- a/src/utility/math/solve_pnp.hpp +++ b/src/utility/math/solve_pnp.hpp @@ -38,6 +38,7 @@ static auto cast_opencv_matrix(std::array, rows>& source template concept ConvertibleTo = std::is_convertible_v; + /* @Note: Row-Major */ template diff --git a/src/utility/model/armor_detection.hpp b/src/utility/model/armor_detection.hpp index 7c6ccd95..c2c7c917 100644 --- a/src/utility/model/armor_detection.hpp +++ b/src/utility/model/armor_detection.hpp @@ -9,15 +9,16 @@ struct ArmorInferResult { using Point = cv::Point_; using Rect = cv::Rect_; + //(0,1) 顶左，(2,3) 底左，(4,5) 底右，(6,7) 顶右 struct Corners { precision_type lt_x; precision_type lt_y; + precision_type lb_x; + precision_type lb_y; precision_type rb_x; precision_type rb_y; precision_type rt_x; precision_type rt_y; - precision_type lb_x; - precision_type lb_y; auto lt() const noexcept { return Point { lt_x, lt_y }; } auto rb() const noexcept { return Point { rb_x, rb_y }; } auto rt() const noexcept { return Point { rt_x, rt_y }; } diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index dbb08d7f..8f40c7fa 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -2,9 +2,12 @@ #include "utility/panic.hpp" #include "utility/rclcpp/node.details.hpp" +#include + using namespace rmcs::util::visual; -using Marker = visualization_msgs::msg::Marker; +using Marker = visualization_msgs::msg::Marker; +using MarkerArray = visualization_msgs::msg::MarkerArray; struct Armor::Impl { static inline rclcpp::Clock rclcpp_clock { RCL_SYSTEM_TIME }; @@ -12,7 +15,8 @@ struct Armor::Impl { std::unique_ptr config; Marker marker; - std::shared_ptr> rclcpp_pub; + Marker arrow_marker; + std::shared_ptr> rclcpp_pub; explicit Impl(const Config& config) : config(std::make_unique(config)) { @@ -20,7 +24,7 @@ struct Armor::Impl { } static auto create_rclcpp_publisher(Config const& config) - -> std::shared_ptr> { + -> std::shared_ptr> { const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.id }; @@ -28,7 +32,7 @@ struct Armor::Impl { util::panic("Rclcpp node details are required in config to create publisher."); } - return config.rclcpp.details->make_pub(topic_name, qos::debug); + return config.rclcpp.details->make_pub(topic_name, qos::debug); } auto initialize() -> void { @@ -42,6 +46,7 @@ struct Armor::Impl { marker.id = 0; marker.type = Marker::CUBE; marker.action = Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); // ref: "https://www.robomaster.com/zh-CN/products/components/detail/149" /* */ if (DeviceIds::kSmallArmorDevices().contains(config->device)) { @@ -57,23 +62,53 @@ struct Armor::Impl { } else if (config->camp == CampColor::BLUE) { marker.color.r = 0., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; } else { - util::panic("Please specify a valid armor color"); + marker.color.r = 1., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; + } + + arrow_marker.header.frame_id = config->tf; + arrow_marker.ns = config->id + std::string("_arrow"); + arrow_marker.id = 1; + arrow_marker.type = Marker::ARROW; + arrow_marker.action = Marker::ADD; + arrow_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + + arrow_marker.scale.x = 0.2; + arrow_marker.scale.y = 0.01; + arrow_marker.scale.z = 0.01; + + /* */ if (config->camp == CampColor::RED) { + arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 0., + arrow_marker.color.a = 1.; + } else if (config->camp == CampColor::BLUE) { + arrow_marker.color.r = 0., arrow_marker.color.g = 0., arrow_marker.color.b = 1., + arrow_marker.color.a = 1.; + } else { + arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 1., + arrow_marker.color.a = 1.; } } auto set_external_rclcpp_publisher( - std::shared_ptr> const& _rclcpp_pub) -> void { + std::shared_ptr> const& _rclcpp_pub) -> void { rclcpp_pub = _rclcpp_pub; } auto update() noexcept -> void { if (!rclcpp_pub) { const auto topic_name { config->rclcpp.get_pub_topic_prefix() + config->id }; - rclcpp_pub = config->rclcpp.details->make_pub(topic_name, qos::debug); + rclcpp_pub = config->rclcpp.details->make_pub(topic_name, qos::debug); } - marker.header.stamp = rclcpp_clock.now(); - rclcpp_pub->publish(marker); + MarkerArray visual_marker; + const auto current_stamp = rclcpp_clock.now(); + marker.header.stamp = current_stamp; + arrow_marker.header.stamp = current_stamp; + + arrow_marker.pose = marker.pose; + visual_marker.markers.emplace_back(marker); + visual_marker.markers.emplace_back(arrow_marker); + + rclcpp_pub->publish(visual_marker); } auto move(const Translation& t, const Orientation& q) noexcept { @@ -92,7 +127,7 @@ Armor::Armor(const Config& config) noexcept : pimpl { std::make_unique(config) } { } Armor::Armor( - Config const& config, std::shared_ptr> const& publisher) noexcept + Config const& config, std::shared_ptr> const& publisher) noexcept : pimpl { std::make_unique(config) } { pimpl->set_external_rclcpp_publisher(publisher); } diff --git a/src/utility/rclcpp/visual/armor.hpp b/src/utility/rclcpp/visual/armor.hpp index c1f3c4da..cd7131d5 100644 --- a/src/utility/rclcpp/visual/armor.hpp +++ b/src/utility/rclcpp/visual/armor.hpp @@ -4,7 +4,7 @@ #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" #include -#include +#include namespace rmcs::util::visual { @@ -22,7 +22,7 @@ struct Armor : public Movable { explicit Armor(const Config&) noexcept; explicit Armor(Config const&, - std::shared_ptr> const&) noexcept; + std::shared_ptr> const&) noexcept; ~Armor() noexcept; Armor(const Armor&) = delete; diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 3e3110b4..a80f6491 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -54,22 +54,22 @@ constexpr double kSmallArmorWidth = 0.135; constexpr std::array kLargeArmorShapeOpenCV { Point3d { -0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left - Point3d { 0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right Point3d { 0.5 * kLargeArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { 0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right Point3d { -0.5 * kLargeArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left }; constexpr std::array kSmallArmorShapeOpenCV { Point3d { -0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-left - Point3d { 0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right Point3d { 0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight, 0.0 }, // Top-right + Point3d { 0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 }, // Bottom-right Point3d { -0.5 * kSmallArmorWidth, 0.5 * kLightBarHeight, -0.0 } // Bottom-left }; constexpr std::array kLargeArmorShapeRos { Point3d { 0.0, 0.115, 0.028 }, // Top-left - Point3d { 0.0, -0.115, -0.028 }, // Bottom-right Point3d { 0.0, -0.115, 0.028 }, // Top-right + Point3d { 0.0, -0.115, -0.028 }, // Bottom-right Point3d { 0.0, 0.115, -0.028 } // Bottom-left }; constexpr std::array kSmallArmorShapeRos { @@ -78,5 +78,4 @@ constexpr std::array kSmallArmorShapeRos { Point3d { 0.0, -0.0675, -0.028 }, // Bottom-right Point3d { 0.0, 0.0675, -0.028 } // Bottom-left }; - } From c6f08049bd95b142527cd2ff19c621debdbb9e44 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 12 Dec 2025 05:15:30 +0800 Subject: [PATCH 08/36] fix(coords, pnp): correct coordinate system transformation error Resolved the critical issue in coordinate system transformation logic that was causing inaccuracies in pose estimation. The fix ensures the proper conversion between the OpenCV and ROS camera coordinate definitions, resulting in a correct and stable output from the PnP (Perspective-n-Point) solver. The PnP pipeline is now fully operational and yielding expected results. --- CMakeLists.txt | 2 +- src/module/pose_estimator/pnp_solution.cpp | 88 +++----------------- src/utility/math/conversion.hpp | 93 +++++----------------- 3 files changed, 33 insertions(+), 150 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fba3c12..50225817 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 23) set(CMAKE_BUILD_TYPE "Release") add_compile_options(-Wall -Wextra -Wpedantic -O3) -add_definitions(-DOPENCV_DISABLE_EIGEN_TENSOR_SUPPORT) + ## 依赖查找 find_package(yaml-cpp REQUIRED) find_package(OpenCV 4.5 REQUIRED) diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/module/pose_estimator/pnp_solution.cpp index 58c09bfd..11049368 100644 --- a/src/module/pose_estimator/pnp_solution.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -4,26 +4,15 @@ #include "utility/math/solve_pnp.hpp" #include #include -#include -#include #include +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include + using namespace rmcs::util; auto PnpSolution::solve() noexcept -> void { - // const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); - // const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); - - auto camera_matrix = Eigen::Matrix {}; - camera_matrix << 1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03, - 5.645821718351237e+02, 0, 0, 1; - - auto distort_coeff = Eigen::Matrix {}; - distort_coeff << -0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582; - - auto camera_matrix_ = cv::Mat {}; - auto distort_coeff_ = cv::Mat {}; - cv::eigen2cv(camera_matrix, camera_matrix_); - cv::eigen2cv(distort_coeff, distort_coeff_); + const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); + const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); const auto armor_shape = std::ranges::to(input.armor_shape | std::views::transform([](const Point3d& point) { return point.make(); })); @@ -31,91 +20,38 @@ auto PnpSolution::solve() noexcept -> void { const auto armor_detection = std::ranges::to(input.armor_detection | std::views::transform([](const Point2d& point) { return point.make(); })); - // fmt::print("K:\n"); - // for (const auto& row : input.camera_matrix) { - // fmt::print(" [{:f}, {:f}, {:f}]\n", row[0], row[1], row[2]); - // } - // fmt::print("D: [{:f}, {:f}, {:f}, {:f}, {:f}]\n", input.distort_coeff[0], - // input.distort_coeff[1], input.distort_coeff[2], input.distort_coeff[3], - // input.distort_coeff[4]); - // - // std::cout << "armor_shape:\n"; - // for (const auto& p : armor_shape) { - // std::cout << " (" << p.x << ", " << p.y << ", " << p.z << ")\n"; - // } - // std::cout << "armor_detection:\n"; - // for (const auto& p : armor_detection) { - // std::cout << " (" << p.x << ", " << p.y << ")\n"; - // } - auto rota_vec = cv::Vec3d {}; auto tran_vec = cv::Vec3d {}; - cv::solvePnP(armor_shape, armor_detection, camera_matrix_, distort_coeff_, rota_vec, tran_vec, + cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); auto tran_vec_eigen_opencv = Eigen::Vector3d {}; cv::cv2eigen(tran_vec, tran_vec_eigen_opencv); - // tran_vec_eigen_opencv.x() = tran_vec[0]; - // tran_vec_eigen_opencv.y() = tran_vec[1]; - // tran_vec_eigen_opencv.z() = tran_vec[2]; auto rotation_opencv = cv::Mat {}; cv::Rodrigues(rota_vec, rotation_opencv); - auto rotation_eigen_opencv = Eigen::Matrix3d {}; cv::cv2eigen(rotation_opencv, rotation_eigen_opencv); - // rotation_eigen_opencv << // Major - // rotation_opencv.at(0, 0), // [0,0] - // rotation_opencv.at(0, 1), // [0,1] - // rotation_opencv.at(0, 2), // [0,2] - // rotation_opencv.at(1, 0), // [1,0] - // rotation_opencv.at(1, 1), // [1,1] - // rotation_opencv.at(1, 2), // [1,2] - // rotation_opencv.at(2, 0), // [2,0] - // rotation_opencv.at(2, 1), // [2,1] - // rotation_opencv.at(2, 2); // [2,2] - // - // rotation_eigen_opencv.transpose(); - // - // auto [rotation_eigen_ros, tran_vec_eigen_ros] = - // cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); - // auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); - result.genre = input.genre; - result.color = input.color; - // result.translation = tran_vec_eigen_ros; - // result.orientation = orientation_ros; - result.translation = tran_vec_eigen_opencv; - result.orientation = Eigen::Quaterniond(rotation_eigen_opencv).normalized(); + result.genre = input.genre; + result.color = input.color; + result.translation = opencv2ros_position(tran_vec_eigen_opencv); + result.orientation = + Eigen::Quaterniond(ros2opencv_rotation(rotation_eigen_opencv)).normalized(); } auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { using namespace visual; if (!visualized_armor) { auto const config = Armor::Config { - .rclcpp = visual_node, .device = result.genre, .camp = result.color, .id = "solved_pnp_armor", .tf = "camera_link", }; - visualized_armor = std::make_unique(config); - auto const target_config = Armor::Config { - - .rclcpp = visual_node, - .device = result.genre, - .camp = result.color, - .id = "target_pnp_armor", - .tf = "camera_link", - }; - target_armor = std::make_unique(target_config); + visualized_armor = std::make_unique(config); } visualized_armor->move(result.translation, result.orientation); visualized_armor->update(); - - auto tran = Translation { 0, 0, 1.75 }; - auto orientation = Orientation { 0, 0.7071, 0, 0.7071 }; - target_armor->move(tran, orientation); - target_armor->update(); } diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp index fc6461f9..4c5fed25 100644 --- a/src/utility/math/conversion.hpp +++ b/src/utility/math/conversion.hpp @@ -1,87 +1,34 @@ #pragma once -#include -#include -#include "utility/math/linear.hpp" +#include namespace rmcs::util { -// 坐标变换公式： -// P_C_cv = R_cv * P_W + t_cv -// P_C_ros = T * (R_cv * P_W + t_cv) = (T * R_cv) * P_W + (T * t_cv) -// 因此： -// R_ros = T * R_cv -// t_ros = T * t_cv -// 其中 T 为从 OpenCV 光学系到 ROS camera_link 的基变换矩阵。 - -/// 生成从源坐标系到目标坐标系的转换矩阵 T，使得 v_target = T * v_source。 -/// 输入轴向量支持 linear.hpp 中的自定义三维向量或 Eigen::Vector3d，并保证为正交单位向量。 -template -inline auto make_basis_transform(const VecSrc& x_src, const VecSrc& y_src, const VecSrc& z_src, - const VecDst& x_dst, const VecDst& y_dst, const VecDst& z_dst) -> Eigen::Matrix3d { - - auto to_eigen = [](const auto& v) -> Eigen::Vector3d { - using T = std::decay_t; - if constexpr (rmcs::translation_object_trait) { - return { v.x(), v.y(), v.z() }; - } else { - return { v.x, v.y, v.z }; - } - }; - - const auto Xs = to_eigen(x_src); - const auto Ys = to_eigen(y_src); - const auto Zs = to_eigen(z_src); - const auto Xd = to_eigen(x_dst); - const auto Yd = to_eigen(y_dst); - const auto Zd = to_eigen(z_dst); - - [[maybe_unused]] auto is_unit = [](const Eigen::Vector3d& v) { - return std::abs(v.norm() - 1.0) < 1e-6; - }; - [[maybe_unused]] auto is_orth = [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) { - return std::abs(a.dot(b)) < 1e-6; - }; - - assert(is_unit(Xs) && is_unit(Ys) && is_unit(Zs) && "source must be unit length"); - assert(is_unit(Xd) && is_unit(Yd) && is_unit(Zd) && "target must be unit length"); - assert(is_orth(Xs, Ys) && is_orth(Ys, Zs) && is_orth(Zs, Xs) && "source must be orthogonal"); - assert(is_orth(Xd, Yd) && is_orth(Yd, Zd) && is_orth(Zd, Xd) && "target must be orthogonal"); - - Eigen::Matrix3d R_src; - R_src.col(0) = Xs; - R_src.col(1) = Ys; - R_src.col(2) = Zs; - - Eigen::Matrix3d R_dst; - R_dst.col(0) = Xd; - R_dst.col(1) = Yd; - R_dst.col(2) = Zd; - - // 将源坐标的分量映射到目标坐标分量 - return R_dst.transpose() * R_src; +static inline auto opencv2ros_position(const Eigen::Vector3d& position) -> Eigen::Vector3d { + auto result = Eigen::Vector3d(position.z(), -position.x(), -position.y()); + return result; } -/// OpenCV 光学坐标系 (x:右, y:下, z:前) -> ROS camera_link (x:前, y:左, z:上) -inline auto make_cv_optical_to_ros_camera_link() -> Eigen::Matrix3d { - using V = Eigen::Vector3d; - const V x_cv { 1., 0., 0. }; - const V y_cv { 0., 1., 0. }; - const V z_cv { 0., 0., 1. }; - - const V x_ros { 0., 0., 1. }; // 前 - const V y_ros { -1., 0., 0. }; // 左 - const V z_ros { 0., -1., 0. }; // 上 +static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotation_matrix) { + Eigen::Matrix3d t; + t << 0, 0, 1, // + -1, 0, 0, // + 0, -1, 0; // + return t * rotation_matrix * t.transpose(); +} - return make_basis_transform(x_cv, y_cv, z_cv, x_ros, y_ros, z_ros); +static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& position) { + auto result = Eigen::Vector3d(-position.y(), -position.z(), position.x()); + return result; } -/// 将 solvePnP 的输出 (OpenCV 光学坐标系) 转换为 ROS camera_link 坐标系 -inline auto cv_optical_to_ros_camera_link(const Eigen::Matrix3d& R_cv, const Eigen::Vector3d& t_cv) - -> std::pair { - const auto T = make_cv_optical_to_ros_camera_link(); - return { T * R_cv, T * t_cv }; +static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotation_matrix) { + Eigen::Matrix3d t; + t << 0, 0, 1, // + -1, 0, 0, // + 0, -1, 0; // + return t.transpose() * rotation_matrix * t; } } From ead165c9acb8db43892db629025da53e8b781e80 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 13 Dec 2025 07:45:18 +0800 Subject: [PATCH 09/36] refactor(pnp): decouple PnP solver implementation and visualization The PnP (Perspective-n-Point) solver implementation has been separated from its visualization logic. - The `solve_pnp` module now strictly focuses on calculating the 3D pose (rotation and translation) and returns the numerical result. - All drawing, rendering, and coordinate frame visualization code has been moved to a dedicated `visualization` or `debug` module. --- config/config.yaml | 4 +- src/kernel/pose_estimator.cpp | 83 ++++++++--------- src/kernel/pose_estimator.hpp | 10 +-- src/kernel/visualization.cpp | 38 ++++++-- src/kernel/visualization.hpp | 8 +- .../debug/visualization/armor_visualizer.cpp | 89 +++++++++++++++++++ .../debug/visualization/armor_visualizer.hpp | 20 +++++ src/runtime.cpp | 11 +-- .../math/solve_pnp}/pnp_solution.cpp | 16 ---- .../math/solve_pnp}/pnp_solution.hpp | 7 -- src/utility/rclcpp/visual/armor.cpp | 29 ++---- src/utility/rclcpp/visual/armor.hpp | 7 +- src/utility/robot/armor.hpp | 22 ++++- 13 files changed, 231 insertions(+), 113 deletions(-) create mode 100644 src/module/debug/visualization/armor_visualizer.cpp create mode 100644 src/module/debug/visualization/armor_visualizer.hpp rename src/{module/pose_estimator => utility/math/solve_pnp}/pnp_solution.cpp (74%) rename src/{module/pose_estimator => utility/math/solve_pnp}/pnp_solution.hpp (76%) diff --git a/config/config.yaml b/config/config.yaml index 0cac0678..8603675c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,6 +1,5 @@ use_visualization: true use_painted_image: true -open_solved_pnp_visualization: true capturer: show_loss_framerate: false @@ -67,6 +66,7 @@ pose_estimator: visualization: framerate: 60 - monitor_host: "192.168.2.154" + monitor_host: "192.168.31.79" monitor_port: "5000" stream_type: "RTP_JEPG" + show_armors: true diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 1b163192..9e3a56b9 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -3,6 +3,8 @@ #include "kernel/transform_tree.hpp" #include "utility/logging/printer.hpp" #include "utility/math/solve_pnp.hpp" +#include "utility/math/solve_pnp/pnp_solution.hpp" +#include "utility/rclcpp/visual/armor.hpp" #include "utility/serializable.hpp" #include "utility/yaml/tf.hpp" @@ -17,16 +19,17 @@ struct PoseEstimator::Impl { std::array camera_matrix; std::array distort_coeff; - // clang-format off constexpr static std::tuple metas { - &Config::camera_matrix, "camera_matrix", - &Config::distort_coeff, "distort_coeff", + &Config::camera_matrix, + "camera_matrix", + &Config::distort_coeff, + "distort_coeff", }; - // clang-format on }; - Config config; - std::vector pnp_solutions {}; + Config config; + PnpSolution pnp_solution {}; + std::vector> visual_armors {}; Printer log { "PoseEstimator" }; @@ -35,7 +38,6 @@ struct PoseEstimator::Impl { if (!result.has_value()) { return std::unexpected { result.error() }; } - { auto result = serialize_from(yaml["transforms"]); if (!result.has_value() @@ -44,26 +46,28 @@ struct PoseEstimator::Impl { + util::to_string(result.error()) }; } } + { + pnp_solution.input.camera_matrix = + reshape_array(config.camera_matrix); + pnp_solution.input.distort_coeff = + reshape_array(config.distort_coeff); + } return {}; } catch (const std::exception& e) { return std::unexpected { e.what() }; } - auto solve_pnp(std::optional> const& armors) noexcept -> void { - if (!armors.has_value()) return; - + auto solve_pnp(std::optional> const& armors) noexcept + -> std::optional> { + if (!armors.has_value()) return std::nullopt; auto _armors = (*armors); - pnp_solutions.reserve(_armors.size()); - pnp_solutions.resize(_armors.size()); - auto color = [](ArmorColor const& color) -> CampColor { - if (color == ArmorColor::BLUE) return CampColor::BLUE; - if (color == ArmorColor::RED) return CampColor::RED; - return CampColor::UNKNOWN; - }; + auto new_size = _armors.size(); + visual_armors.reserve(new_size); + visual_armors.resize(new_size); - auto shape = [](ArmorShape shape) -> std::array { + auto armor_shape = [](ArmorShape shape) { if (shape == ArmorShape::SMALL) { return rmcs::kSmallArmorShapeOpenCV; } else { @@ -71,30 +75,30 @@ struct PoseEstimator::Impl { } }; - auto const _camera_matrix = reshape_array(config.camera_matrix); - auto const _distort_coeff = reshape_array(config.distort_coeff); + auto armors_in_camera = std::vector {}; + + std::ranges::for_each(_armors | std::views::enumerate, + [&armors_in_camera, &armor_shape, this](auto const& item) { + auto [i, armor] = item; - std::ranges::for_each(std::views::zip(_armors, pnp_solutions), - [&color, &shape, &_camera_matrix, &_distort_coeff](auto&& pair) { - auto const& [armor, pnp_solution] = pair; + pnp_solution.input.armor_shape = armor_shape(armor.shape); + pnp_solution.input.genre = armor.genre; + pnp_solution.input.color = armor_color2camp_color(armor.color); + std::ranges::copy(armor.corners(), pnp_solution.input.armor_detection.begin()); - PnpSolution& solution = const_cast(pnp_solution); + pnp_solution.solve(); - solution.input.armor_shape = shape(armor.shape); - solution.input.genre = armor.genre; - solution.input.color = color(armor.color); - std::ranges::copy(armor.corners(), solution.input.armor_detection.begin()); - solution.input.camera_matrix = _camera_matrix; - solution.input.distort_coeff = _distort_coeff; + auto armor_3d = Armor3D {}; + armor_3d.genre = pnp_solution.result.genre; + armor_3d.color = camp_color2armor_color(pnp_solution.result.color); + armor_3d.id = i; + pnp_solution.result.translation.copy_to(armor_3d.translation); + pnp_solution.result.orientation.copy_to(armor_3d.orientation); - solution.solve(); + armors_in_camera.emplace_back(armor_3d); }); - } - auto visualize(RclcppNode& visual_node) -> void { - for (auto& solution : pnp_solutions) { - solution.visualize(visual_node); - } + return armors_in_camera; } }; @@ -103,14 +107,11 @@ auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept return pimpl->initialize(yaml); } -void PoseEstimator::solve_pnp(std::optional> const& armors) const noexcept { +auto PoseEstimator::solve_pnp(std::optional> const& armors) const noexcept + -> std::optional> { return pimpl->solve_pnp(armors); } -auto PoseEstimator::visualize(RclcppNode& visual_node) -> void { - return pimpl->visualize(visual_node); -} - PoseEstimator::PoseEstimator() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 2cf0a741..37f62b74 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -1,6 +1,5 @@ #pragma once -#include "module/pose_estimator/pnp_solution.hpp" #include "utility/math/linear.hpp" #include "utility/pimpl.hpp" #include "utility/rclcpp/node.hpp" @@ -14,15 +13,12 @@ class PoseEstimator { RMCS_PIMPL_DEFINITION(PoseEstimator) public: - using PnpSolution = util::PnpSolution; - using RclcppNode = util::RclcppNode; + using RclcppNode = util::RclcppNode; auto initialize(const YAML::Node&) noexcept -> std::expected; - auto solve_pnp(std::optional> const&) const noexcept -> void; - - auto visualize(RclcppNode& visual_node) -> void; - + auto solve_pnp(std::optional> const&) const noexcept + -> std::optional>; auto update_imu_link(const Orientation&) noexcept -> void; }; diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 297bc629..2d159539 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -1,13 +1,15 @@ #include "visualization.hpp" -#include "module/debug/visualization/stream_session.hpp" +#include + +#include "module/debug/visualization/armor_visualizer.hpp" +#include "module/debug/visualization/stream_session.hpp" #include "utility/image/image.details.hpp" #include "utility/logging/printer.hpp" #include "utility/serializable.hpp" -#include - using namespace rmcs::kernel; +using namespace rmcs::util; constexpr std::array kVideoTypes { "RTP_JEPG", @@ -27,6 +29,8 @@ struct Visualization::Impl { util::string_t stream_type = "RTP_JEPG"; + bool show_armors = false; + static constexpr auto metas = std::tuple { &Config::framerate, "framerate", @@ -36,6 +40,8 @@ struct Visualization::Impl { "monitor_port", &Config::stream_type, "stream_type", + &Config::show_armors, + "show_armors", }; }; @@ -47,9 +53,14 @@ struct Visualization::Impl { bool is_initialized = false; bool size_determined = false; - Impl() noexcept { session = std::make_unique(); } + std::unique_ptr armor_visualizer; + + Impl() noexcept { + session = std::make_unique(); + armor_visualizer = std::make_unique(); + } - auto initialize(const YAML::Node& yaml) noexcept -> NormalResult { + auto initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> NormalResult { auto config = Config {}; auto result = config.serialize(yaml); if (!result.has_value()) { @@ -67,6 +78,9 @@ struct Visualization::Impl { } else { return std::unexpected { "Unknown video type: " + config.stream_type }; } + + armor_visualizer->initialize(visual_node); + is_initialized = true; return {}; } @@ -114,11 +128,16 @@ struct Visualization::Impl { return session->push_frame(mat); } + + auto visualize_armors(std::span const& armors) const + -> std::expected { + return armor_visualizer->visualize(armors); + } }; -auto Visualization::initialize(const YAML::Node& yaml) noexcept +auto Visualization::initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> std::expected { - return pimpl->initialize(yaml); + return pimpl->initialize(yaml, visual_node); } auto Visualization::initialized() const noexcept -> bool { return pimpl->initialized(); } @@ -127,6 +146,11 @@ auto Visualization::send_image(const Image& image) noexcept -> bool { return pimpl->send_image(image); } +auto Visualization::visualize_armors(std::span const& armors) const + -> std::expected { + return pimpl->visualize_armors(armors); +} + Visualization::Visualization() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index 419e2a97..0a1307eb 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -1,5 +1,7 @@ #pragma once #include "utility/image/image.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" #include #include @@ -17,11 +19,15 @@ class Visualization { } public: - auto initialize(const YAML::Node&) noexcept -> std::expected; + auto initialize(const YAML::Node& yaml, util::RclcppNode& visual_node) noexcept + -> std::expected; auto initialized() const noexcept -> bool; auto send_image(const Image&) noexcept -> bool; + + auto visualize_armors(std::span const& armors) const + -> std::expected; }; } diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp new file mode 100644 index 00000000..1e3b8e6e --- /dev/null +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -0,0 +1,89 @@ +#include "armor_visualizer.hpp" + +#include "utility/rclcpp/visual/armor.hpp" + +using namespace rmcs::debug; +using VisualArmor = rmcs::util::visual::Armor; + +struct ArmorShadow { + decltype(rmcs::Armor3D::genre) genre; + decltype(rmcs::Armor3D::color) color; + decltype(rmcs::Armor3D::id) id; + + bool operator==(ArmorShadow const& other) const = default; + bool operator!=(ArmorShadow const& other) const { return !(*this == other); } +}; + +struct ArmorVisualizer::Impl final { + auto initialize(util::RclcppNode& visual_node) noexcept -> void { + node = std::ref(visual_node); + } + + auto visualize(std::span const& _armors) -> std::expected { + if (!node.has_value()) { + return std::unexpected { "Visual node need to be initialized!" }; + } + + auto new_size = _armors.size(); + visual_armors.reserve(new_size); + current_armors.reserve(new_size); + visual_armors.resize(new_size); + current_armors.resize(new_size); + + for (size_t i = 0; i < new_size; i++) { + auto const& input = _armors[i]; + auto& armor_ptr = visual_armors[i]; + auto& shadow = current_armors[i]; + + bool changed = !armor_ptr || needs_rebuild(shadow, input); + + if (changed) { + auto const config = VisualArmor::Config { + .rclcpp = node.value().get(), + .device = input.genre, + .camp = camp(input.color), + .id = input.id, + .name = "solved_pnp_armor", + .tf = "camera_link", + }; + + armor_ptr = std::make_unique(config); + + shadow.genre = input.genre; + shadow.color = input.color; + } + + armor_ptr->move(input.translation, input.orientation); + armor_ptr->update(); + } + + return {}; + } + + static auto camp(ArmorColor const& color) -> CampColor { + if (color == ArmorColor::BLUE) return CampColor::BLUE; + if (color == ArmorColor::RED) return CampColor::RED; + return CampColor::UNKNOWN; + }; + + static bool needs_rebuild(ArmorShadow shadow, Armor3D const& input) { + return shadow.genre != input.genre || shadow.color != input.color || shadow.id != input.id; + } + + std::optional> node; + std::vector current_armors; + std::vector> visual_armors; +}; + +auto ArmorVisualizer::initialize(util::RclcppNode& visual_node) noexcept -> void { + return pimpl->initialize(visual_node); +} + +auto ArmorVisualizer::visualize(std::span const& armors) + -> std::expected { + return pimpl->visualize(armors); +} + +ArmorVisualizer::ArmorVisualizer() noexcept + : pimpl { std::make_unique() } { }; +ArmorVisualizer::~ArmorVisualizer() noexcept = default; diff --git a/src/module/debug/visualization/armor_visualizer.hpp b/src/module/debug/visualization/armor_visualizer.hpp new file mode 100644 index 00000000..9e4d249c --- /dev/null +++ b/src/module/debug/visualization/armor_visualizer.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +#include "utility/pimpl.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/robot/armor.hpp" + +namespace rmcs::debug { + +class ArmorVisualizer { + + RMCS_PIMPL_DEFINITION(ArmorVisualizer) + +public: + auto initialize(util::RclcppNode&) noexcept -> void; + + auto visualize(std::span const&) -> std::expected; +}; +} diff --git a/src/runtime.cpp b/src/runtime.cpp index 375d224c..bb24d612 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -48,10 +48,9 @@ auto main() -> int { /// Configure /// - auto configuration = util::configuration(); - auto use_visualization = configuration["use_visualization"].as(); - auto use_painted_image = configuration["use_painted_image"].as(); - auto open_solved_pnp_visualization = configuration["open_solved_pnp_visualization"].as(); + auto configuration = util::configuration(); + auto use_visualization = configuration["use_visualization"].as(); + auto use_painted_image = configuration["use_painted_image"].as(); // CAPTURER { @@ -79,12 +78,10 @@ auto main() -> int { // VISUALIZATION if (use_visualization) { auto config = configuration["visualization"]; - auto result = visualization.initialize(config); + auto result = visualization.initialize(config, rclcpp_node); handle_result("visualization", result); } - rmcs::Printer printer { "rmcs_auto_aim" }; - for (;;) { if (!util::get_running()) [[unlikely]] break; diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp similarity index 74% rename from src/module/pose_estimator/pnp_solution.cpp rename to src/utility/math/solve_pnp/pnp_solution.cpp index 11049368..68ce8859 100644 --- a/src/module/pose_estimator/pnp_solution.cpp +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -39,19 +39,3 @@ auto PnpSolution::solve() noexcept -> void { result.orientation = Eigen::Quaterniond(ros2opencv_rotation(rotation_eigen_opencv)).normalized(); } - -auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { - using namespace visual; - if (!visualized_armor) { - auto const config = Armor::Config { - .rclcpp = visual_node, - .device = result.genre, - .camp = result.color, - .id = "solved_pnp_armor", - .tf = "camera_link", - }; - visualized_armor = std::make_unique(config); - } - visualized_armor->move(result.translation, result.orientation); - visualized_armor->update(); -} diff --git a/src/module/pose_estimator/pnp_solution.hpp b/src/utility/math/solve_pnp/pnp_solution.hpp similarity index 76% rename from src/module/pose_estimator/pnp_solution.hpp rename to src/utility/math/solve_pnp/pnp_solution.hpp index 90d2eff1..638d1037 100644 --- a/src/module/pose_estimator/pnp_solution.hpp +++ b/src/utility/math/solve_pnp/pnp_solution.hpp @@ -4,8 +4,6 @@ #include "utility/math/linear.hpp" #include "utility/math/point.hpp" -#include "utility/rclcpp/node.hpp" -#include "utility/rclcpp/visual/armor.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" @@ -28,13 +26,8 @@ struct PnpSolution { CampColor color; } result; - std::unique_ptr visualized_armor; - std::unique_ptr target_armor; - PnpSolution() noexcept = default; auto solve() noexcept -> void; - - auto visualize(RclcppNode&) noexcept -> void; }; } diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index 8f40c7fa..c78b8374 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -26,7 +26,7 @@ struct Armor::Impl { static auto create_rclcpp_publisher(Config const& config) -> std::shared_ptr> { - const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.id }; + const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.name }; if (!config.rclcpp.details) { util::panic("Rclcpp node details are required in config to create publisher."); @@ -36,14 +36,14 @@ struct Armor::Impl { } auto initialize() -> void { - if (!prefix::check_naming(config->id) || !prefix::check_naming(config->tf)) { - util::panic( - std::format("Not a valid naming for armor id or tf: {}", prefix::naming_standard)); + if (!prefix::check_naming(config->name) || !prefix::check_naming(config->tf)) { + util::panic(std::format( + "Not a valid naming for armor name or tf: {}", prefix::naming_standard)); } marker.header.frame_id = config->tf; - marker.ns = config->id; - marker.id = 0; + marker.ns = config->name; + marker.id = config->id; marker.type = Marker::CUBE; marker.action = Marker::ADD; marker.lifetime = rclcpp::Duration::from_seconds(0.1); @@ -66,8 +66,8 @@ struct Armor::Impl { } arrow_marker.header.frame_id = config->tf; - arrow_marker.ns = config->id + std::string("_arrow"); - arrow_marker.id = 1; + arrow_marker.ns = config->name + std::string("_arrow"); + arrow_marker.id = config->id; arrow_marker.type = Marker::ARROW; arrow_marker.action = Marker::ADD; arrow_marker.lifetime = rclcpp::Duration::from_seconds(0.1); @@ -88,14 +88,9 @@ struct Armor::Impl { } } - auto set_external_rclcpp_publisher( - std::shared_ptr> const& _rclcpp_pub) -> void { - rclcpp_pub = _rclcpp_pub; - } - auto update() noexcept -> void { if (!rclcpp_pub) { - const auto topic_name { config->rclcpp.get_pub_topic_prefix() + config->id }; + const auto topic_name { config->rclcpp.get_pub_topic_prefix() + config->name }; rclcpp_pub = config->rclcpp.details->make_pub(topic_name, qos::debug); } @@ -126,10 +121,4 @@ auto Armor::impl_move(const Translation& t, const Orientation& q) noexcept -> vo Armor::Armor(const Config& config) noexcept : pimpl { std::make_unique(config) } { } -Armor::Armor( - Config const& config, std::shared_ptr> const& publisher) noexcept - : pimpl { std::make_unique(config) } { - pimpl->set_external_rclcpp_publisher(publisher); -} - Armor::~Armor() noexcept = default; diff --git a/src/utility/rclcpp/visual/armor.hpp b/src/utility/rclcpp/visual/armor.hpp index cd7131d5..e09f1a4f 100644 --- a/src/utility/rclcpp/visual/armor.hpp +++ b/src/utility/rclcpp/visual/armor.hpp @@ -4,7 +4,6 @@ #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" #include -#include namespace rmcs::util::visual { @@ -16,13 +15,13 @@ struct Armor : public Movable { DeviceId device; CampColor camp; - std::string id; + int id; + std::string name; std::string tf; }; explicit Armor(const Config&) noexcept; - explicit Armor(Config const&, - std::shared_ptr> const&) noexcept; + ~Armor() noexcept; Armor(const Armor&) = delete; diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index a80f6491..4ad1c76e 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -1,6 +1,8 @@ #pragma once #include "utility/math/point.hpp" +#include "utility/robot/color.hpp" #include "utility/robot/id.hpp" +#include #include #include @@ -11,6 +13,17 @@ constexpr auto get_enum_name(ArmorColor color) noexcept { constexpr std::array details { "DARK", "RED", "BLUE", "MIX" }; return details[std::to_underlying(color)]; } +inline constexpr auto armor_color2camp_color(ArmorColor const& color) -> CampColor { + if (color == ArmorColor::BLUE) return CampColor::BLUE; + if (color == ArmorColor::RED) return CampColor::RED; + return CampColor::UNKNOWN; +}; + +inline constexpr auto camp_color2armor_color(CampColor const& color) -> ArmorColor { + if (color == CampColor::BLUE) return ArmorColor::BLUE; + if (color == CampColor::RED) return ArmorColor::RED; + return ArmorColor::MIX; +}; enum class ArmorShape : bool { LARGE, SMALL }; constexpr auto get_enum_name(ArmorShape shape) noexcept { @@ -43,7 +56,14 @@ struct Armor2D { } }; -struct Armor3D { }; +struct Armor3D { + ArmorGenre genre; + ArmorColor color; + int id; + + Eigen::Vector3d translation; + Eigen::Quaterniond orientation; +}; struct Armor { }; using Armors = std::vector; From 1ee1d52bbd754cea7351ae48d68b59e038ae76b7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 14 Dec 2025 06:48:23 +0800 Subject: [PATCH 10/36] test(pnp): add test file with real-world data and establish performance baseline Implemented a dedicated test file utilizing actual captured data to validate the PnP (Perspective-n-Point) solver's output. The current performance baseline using this real-world data is recorded as: - **Distance Error (within 3m):** 8% - **Angular Error:** 15% --- config/config.yaml | 2 +- src/kernel/visualization.cpp | 6 +- src/kernel/visualization.hpp | 3 +- .../debug/visualization/armor_visualizer.cpp | 9 +- .../debug/visualization/armor_visualizer.hpp | 4 +- src/runtime.cpp | 1 - src/utility/math/conversion.hpp | 2 +- src/utility/math/solve_pnp/pnp_solution.cpp | 2 +- test/CMakeLists.txt | 62 +- test/convertion.cpp | 201 ------ test/solve_pnp.cpp | 598 ++++++++---------- tool/visualization.cpp | 11 +- 12 files changed, 307 insertions(+), 594 deletions(-) delete mode 100644 test/convertion.cpp diff --git a/config/config.yaml b/config/config.yaml index 8603675c..2827c1f7 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -66,7 +66,7 @@ pose_estimator: visualization: framerate: 60 - monitor_host: "192.168.31.79" + monitor_host: "192.168.167.79" monitor_port: "5000" stream_type: "RTP_JEPG" show_armors: true diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 2d159539..813e542c 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -129,8 +129,7 @@ struct Visualization::Impl { return session->push_frame(mat); } - auto visualize_armors(std::span const& armors) const - -> std::expected { + auto visualize_armors(std::span const& armors) const -> bool { return armor_visualizer->visualize(armors); } }; @@ -146,8 +145,7 @@ auto Visualization::send_image(const Image& image) noexcept -> bool { return pimpl->send_image(image); } -auto Visualization::visualize_armors(std::span const& armors) const - -> std::expected { +auto Visualization::visualize_armors(std::span const& armors) const -> bool { return pimpl->visualize_armors(armors); } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index 0a1307eb..efd00d9b 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -26,8 +26,7 @@ class Visualization { auto send_image(const Image&) noexcept -> bool; - auto visualize_armors(std::span const& armors) const - -> std::expected; + auto visualize_armors(std::span const& armors) const -> bool; }; } diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index 1e3b8e6e..03fa117c 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -19,9 +19,9 @@ struct ArmorVisualizer::Impl final { node = std::ref(visual_node); } - auto visualize(std::span const& _armors) -> std::expected { + auto visualize(std::span const& _armors) -> bool { if (!node.has_value()) { - return std::unexpected { "Visual node need to be initialized!" }; + return false; } auto new_size = _armors.size(); @@ -57,7 +57,7 @@ struct ArmorVisualizer::Impl final { armor_ptr->update(); } - return {}; + return true; } static auto camp(ArmorColor const& color) -> CampColor { @@ -79,8 +79,7 @@ auto ArmorVisualizer::initialize(util::RclcppNode& visual_node) noexcept -> void return pimpl->initialize(visual_node); } -auto ArmorVisualizer::visualize(std::span const& armors) - -> std::expected { +auto ArmorVisualizer::visualize(std::span const& armors) -> bool { return pimpl->visualize(armors); } diff --git a/src/module/debug/visualization/armor_visualizer.hpp b/src/module/debug/visualization/armor_visualizer.hpp index 9e4d249c..bcc8bea4 100644 --- a/src/module/debug/visualization/armor_visualizer.hpp +++ b/src/module/debug/visualization/armor_visualizer.hpp @@ -1,7 +1,5 @@ #pragma once -#include - #include "utility/pimpl.hpp" #include "utility/rclcpp/node.hpp" #include "utility/robot/armor.hpp" @@ -15,6 +13,6 @@ class ArmorVisualizer { public: auto initialize(util::RclcppNode&) noexcept -> void; - auto visualize(std::span const&) -> std::expected; + auto visualize(std::span const&) -> bool; }; } diff --git a/src/runtime.cpp b/src/runtime.cpp index bb24d612..0b821b0e 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,7 +6,6 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" -#include "utility/logging/printer.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp index 4c5fed25..e48fefed 100644 --- a/src/utility/math/conversion.hpp +++ b/src/utility/math/conversion.hpp @@ -1,7 +1,7 @@ #pragma once -#include +#include namespace rmcs::util { diff --git a/src/utility/math/solve_pnp/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp index 68ce8859..c52fd82f 100644 --- a/src/utility/math/solve_pnp/pnp_solution.cpp +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -37,5 +37,5 @@ auto PnpSolution::solve() noexcept -> void { result.color = input.color; result.translation = opencv2ros_position(tran_vec_eigen_opencv); result.orientation = - Eigen::Quaterniond(ros2opencv_rotation(rotation_eigen_opencv)).normalized(); + Eigen::Quaterniond(opencv2ros_rotation(rotation_eigen_opencv)).normalized(); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7ea2d056..f9c68ad8 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(geometry_msgs REQUIRED) find_package(yaml-cpp REQUIRED) find_package(OpenVINO REQUIRED) find_package(OpenCV 4.5 REQUIRED) +find_package(CURL REQUIRED) include_directories( ${RMCS_SRC_DIR} @@ -68,7 +69,7 @@ target_link_libraries( test_model_infer yaml-cpp::yaml-cpp openvino::runtime - ${OpenCV_LIBS} + ${OpenCV_LIBRARIES} ) # Device Id @@ -81,17 +82,22 @@ ament_add_gtest( ament_add_gtest( test_solve_pnp ${TEST_DIR}/solve_pnp.cpp - ${RMCS_SRC_DIR}/module/pose_estimator/pnp_solution.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp - ${RMCS_SRC_DIR}/utility/panic.cpp + ${RMCS_SRC_DIR}/utility/math/solve_pnp/pnp_solution.cpp + ${RMCS_SRC_DIR}/module/identifier/model.cpp + ${RMCS_SRC_DIR}/utility/image/image.cpp +) +ament_target_dependencies( + test_solve_pnp + rclcpp + visualization_msgs + geometry_msgs ) target_link_libraries( test_solve_pnp ${OpenCV_LIBRARIES} - rclcpp::rclcpp - ${visualization_msgs_LIBRARIES} - ${geometry_msgs_LIBRARIES} + openvino::runtime + yaml-cpp::yaml-cpp + CURL::libcurl ) # Static TF @@ -105,35 +111,29 @@ target_link_libraries( yaml-cpp::yaml-cpp ) -# Conversion -ament_add_gtest( - test_conversion - ${TEST_DIR}/convertion.cpp -) - # Transform Communication ament_add_gtest( test_transform_communication ${TEST_DIR}/transform_communication.cpp ) - -# Visualization -add_executable( - example_visualization - ${TEST_DIR}/visualization.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/visual/posture.cpp - ${RMCS_SRC_DIR}/utility/panic.cpp - ${RMCS_SRC_DIR}/utility/math/solve_armors.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp -) -target_link_libraries( - example_visualization - rclcpp::rclcpp - ${visualization_msgs_LIBRARIES} - ${geometry_msgs_LIBRARIES} -) +if(NOT DEFINED BUILD_VIS_EXAMPLE OR BUILD_VIS_EXAMPLE) + add_executable( + example_visualization + ${TEST_DIR}/visualization.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/visual/posture.cpp + ${RMCS_SRC_DIR}/utility/panic.cpp + ${RMCS_SRC_DIR}/utility/math/solve_armors.cpp + ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp + ) + target_link_libraries( + example_visualization + rclcpp::rclcpp + ${visualization_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ) +endif() if(HIKCAMERA_AVAILABLE) # Hikcamera diff --git a/test/convertion.cpp b/test/convertion.cpp deleted file mode 100644 index c97fb797..00000000 --- a/test/convertion.cpp +++ /dev/null @@ -1,201 +0,0 @@ -#include - -#include "utility/math/conversion.hpp" -#include - -using namespace rmcs::util; - -namespace { - -template -void expect_matrix_near(const MatA& a, const MatB& b, double eps = 1e-9) { - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - EXPECT_NEAR(a(r, c), b(r, c), eps); - } - } -} - -} // namespace - -TEST(Conversion, BasisTransformIdentity) { - Eigen::Vector3d ex { 1, 0, 0 }; - Eigen::Vector3d ey { 0, 1, 0 }; - Eigen::Vector3d ez { 0, 0, 1 }; - - auto T = make_basis_transform(ex, ey, ez, ex, ey, ez); - - expect_matrix_near(T, Eigen::Matrix3d::Identity()); -} - -TEST(Conversion, CvOpticalToRosMatrix) { - const auto T = make_cv_optical_to_ros_camera_link(); - - Eigen::Matrix3d expected; - - // clang-format off - expected << - 0 , 0 , 1, - -1, 0 , 0, - 0 , -1, 0; - // clang-format on - - expect_matrix_near(T, expected); -} - -TEST(Conversion, CvToRosTransformRt) { - Eigen::Matrix3d R_cv = Eigen::Matrix3d::Identity(); - Eigen::Vector3d t_cv { 1., 2., 3. }; - - const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); - - Eigen::Matrix3d T; - - // clang-format off - T << - 0 , 0 , 1, - -1, 0 , 0, - 0 , -1, 0; - // clang-format on - - expect_matrix_near(R_ros, T); - - Eigen::Vector3d expected_t = T * t_cv; - EXPECT_DOUBLE_EQ(t_ros.x(), expected_t.x()); - EXPECT_DOUBLE_EQ(t_ros.y(), expected_t.y()); - EXPECT_DOUBLE_EQ(t_ros.z(), expected_t.z()); -} - -TEST(Conversion, BasisTransformRotate90Z) { - // 源系与目标系：目标系绕 Z 轴 +90 度 - Eigen::Vector3d ex { 1, 0, 0 }; - Eigen::Vector3d ey { 0, 1, 0 }; - Eigen::Vector3d ez { 0, 0, 1 }; - - Eigen::Vector3d ex_rot { 0, 1, 0 }; - Eigen::Vector3d ey_rot { -1, 0, 0 }; - Eigen::Vector3d ez_rot { 0, 0, 1 }; - - auto T = make_basis_transform(ex, ey, ez, ex_rot, ey_rot, ez_rot); - - Eigen::Matrix3d expected, actual; - // clang-format off - actual << - 0 , -1, 0, - 1 , 0, 0, - 0 , 0, 1; - // clang-format on - expected = actual.transpose(); - expect_matrix_near(T, expected); -} - -TEST(Conversion, CvToRosWithRotation) { - // 在 cv 系下绕 Z 轴 90 度，转换后应组合到 T 中 - const auto T = make_cv_optical_to_ros_camera_link(); - - Eigen::AngleAxisd aa_cv(M_PI / 2.0, Eigen::Vector3d::UnitZ()); - Eigen::Matrix3d R_cv = aa_cv.toRotationMatrix(); - Eigen::Vector3d t_cv { 0.5, -0.25, 2.0 }; - - const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); - - Eigen::Matrix3d expected_R = T * R_cv; - expect_matrix_near(R_ros, expected_R); - - Eigen::Vector3d expected_t = T * t_cv; - EXPECT_NEAR(t_ros.x(), expected_t.x(), 1e-12); - EXPECT_NEAR(t_ros.y(), expected_t.y(), 1e-12); - EXPECT_NEAR(t_ros.z(), expected_t.z(), 1e-12); -} - -TEST(Conversion, CvToRosWithNonIdentityBasis) { - // 非标准源/目标基的组合测试 - Eigen::Vector3d src_x { 0, 1, 0 }; // x 指向原来的 y - Eigen::Vector3d src_y { -1, 0, 0 }; // y 指向 -x - Eigen::Vector3d src_z { 0, 0, 1 }; - - Eigen::Vector3d dst_x { 0, 0, 1 }; // x -> 原来的 z - Eigen::Vector3d dst_y { 1, 0, 0 }; // y -> 原来的 x - Eigen::Vector3d dst_z { 0, 1, 0 }; // z -> 原来的 y - - auto T = make_basis_transform(src_x, src_y, src_z, dst_x, dst_y, dst_z); - - Eigen::Matrix3d expected; - // 手工计算：dst^T * src - expected << 0, 0, 1, 0, -1, 0, 1, 0, 0; - - expect_matrix_near(T, expected); -} - -TEST(Conversion, BasisVectorMapping) { - const auto T = make_cv_optical_to_ros_camera_link(); - - const Eigen::Vector3d ex_cv { 1, 0, 0 }; - const Eigen::Vector3d ey_cv { 0, 1, 0 }; - const Eigen::Vector3d ez_cv { 0, 0, 1 }; - - auto ex_ros = T * ex_cv; // 应为 (0,-1,0) - auto ey_ros = T * ey_cv; // 应为 (0,0,-1) - auto ez_ros = T * ez_cv; // 应为 (1,0,0) - - EXPECT_NEAR(ex_ros.x(), 0.0, 1e-12); - EXPECT_NEAR(ex_ros.y(), -1.0, 1e-12); - EXPECT_NEAR(ex_ros.z(), 0.0, 1e-12); - - EXPECT_NEAR(ey_ros.x(), 0.0, 1e-12); - EXPECT_NEAR(ey_ros.y(), 0.0, 1e-12); - EXPECT_NEAR(ey_ros.z(), -1.0, 1e-12); - - EXPECT_NEAR(ez_ros.x(), 1.0, 1e-12); - EXPECT_NEAR(ez_ros.y(), 0.0, 1e-12); - EXPECT_NEAR(ez_ros.z(), 0.0, 1e-12); -} - -TEST(Conversion, ArbitraryVectorTransform) { - Eigen::Vector3d v_cv { 1., 2., 3. }; - auto expected = Eigen::Vector3d { 3., -1., -2. }; - - auto [_, t_ros] = cv_optical_to_ros_camera_link(Eigen::Matrix3d::Identity(), v_cv); - EXPECT_NEAR(t_ros.x(), expected.x(), 1e-12); - EXPECT_NEAR(t_ros.y(), expected.y(), 1e-12); - EXPECT_NEAR(t_ros.z(), expected.z(), 1e-12); - - // TODO:manual calulate; -} - -TEST(Conversion, InverseTransform) { - const auto T = make_cv_optical_to_ros_camera_link(); - Eigen::Matrix3d T_inverse = T.inverse(); - - // 1. 验证逆矩阵是否等于转置 (验证纯旋转特性) - expect_matrix_near(T_inverse, T.transpose()); - - // 2. 验证 T 乘以 T^-1 是否等于单位矩阵 - Eigen::Matrix3d Identity = T * T_inverse; - expect_matrix_near(Identity, Eigen::Matrix3d::Identity()); -} - -TEST(Conversion, CompositeRotation) { - const auto T = make_cv_optical_to_ros_camera_link(); - - // R1: 绕 cv-x 轴 45° - Eigen::AngleAxisd aa_x(M_PI / 4.0, Eigen::Vector3d::UnitX()); - Eigen::Matrix3d R1 = aa_x.toRotationMatrix(); - - // R2: 绕 cv-y 轴 30° - Eigen::AngleAxisd aa_y(M_PI / 6.0, Eigen::Vector3d::UnitY()); - Eigen::Matrix3d R2 = aa_y.toRotationMatrix(); - - // 1. 源系 (CV) 复合旋转 - Eigen::Matrix3d R_cv_comp = R2 * R1; - - // 2. 期望的目标系复合结果 (T * (R2 * R1)) - Eigen::Matrix3d R_ros_expected = T * R_cv_comp; - - // 3. 实际计算 - auto [R_ros_actual, t_ros_acturl] = - cv_optical_to_ros_camera_link(R_cv_comp, Eigen::Vector3d::Zero()); - - // 验证复合结果是否正确 - expect_matrix_near(R_ros_actual, R_ros_expected); -} diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index c2651e37..949b19d8 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -1,398 +1,322 @@ -#include "utility/math/solve_pnp.hpp" -#include "module/pose_estimator/pnp_solution.hpp" -#include "utility/math/linear.hpp" -#include "utility/math/point.hpp" -#include "utility/robot/armor.hpp" +#include // for std::clamp, std::replace +#include +#include +#include // for std::setprecision +#include // for structured output +#include +#include -#include -#include +#include #include -#include +#include +#include + +#include "module/identifier/model.hpp" +#include "utility/image/image.details.hpp" +#include "utility/math/point.hpp" +#include "utility/math/solve_pnp/pnp_solution.hpp" +#include "utility/robot/armor.hpp" -using namespace rmcs; using namespace rmcs::util; +using namespace rmcs; +using Eigen::Quaterniond; using Eigen::Vector2d; using Eigen::Vector3d; -// 辅助函数：创建测试用的相机内参 -PnpSolution::Input create_test_input(double focal_length = 800.0, double cx = 320.0, - double cy = 240.0, const std::array& distort = { 0.0, 0.0, 0.0, 0.0, 0.0 }) { - PnpSolution::Input input; - input.camera_matrix = { { - { focal_length, 0.0, cx }, - { 0.0, focal_length, cy }, - { 0.0, 0.0, 1.0 }, - } }; - input.distort_coeff = distort; - return input; -} - -// 辅助函数：使用 OpenCV 坐标系定义的装甲板 3D 点（z 轴向外） -std::array create_small_armor_shape() { - return rmcs::kSmallArmorShapeOpenCV; -} +// --- 测试数据 --- +struct PnpTestCase { + std::string url; + double expected_distance_m; // 预期的目标距离（米） + double expected_angle_deg; // 预期的偏航角（度，0 或 45） +}; -std::array create_big_armor_shape() { - return rmcs::kLargeArmorShapeOpenCV; -} +// 所有 URL 测试数据 +const std::vector kPnpTestCases = { + { "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m.jpg", 0.5, 0.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m-45degree.jpg", 0.5, 45.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m.jpg", 1.0, 0.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m-45degree.jpg", 1.0, 45.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m.jpg", 2.0, 0.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m-45degree.jpg", 2.0, 45.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m.jpg", 3.0, 0.0 }, + { "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m-45degree.jpg", 3.0, 45.0 }, +}; -// 辅助函数：从 Eigen::Vector2d 创建 Point2d 数组 -std::array create_armor_detection(const std::array& eigen_points) { - auto points = std::array(); - for (size_t i = 0; i < 4; i++) { - points[i] = Point2d(eigen_points[i]); - } - return points; +// --- 网络/模型推理辅助函数 --- +static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp) { + size_t total_size = size * nmemb; + std::vector* buffer = static_cast*>(userp); + buffer->insert(buffer->end(), (uchar*)contents, (uchar*)contents + total_size); + return total_size; } -// 辅助函数：验证四元数是否归一化 -bool is_quaternion_normalized(const Orientation& q, double tolerance = 0.01) { - const auto norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); - return std::abs(norm - 1.0) < tolerance; -} +std::vector download_image_to_buffer(const std::string& url) { + CURL* curl; + CURLcode res; + std::vector buffer; -// 辅助函数：计算两点之间的距离 -double distance(const Translation& a, const Translation& b) { - const auto dx = a.x - b.x; - const auto dy = a.y - b.y; - const auto dz = a.z - b.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} + curl_global_init(CURL_GLOBAL_DEFAULT); + curl = curl_easy_init(); -// ========== 测试用例 ========== + if (curl) { + curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteCallback); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, &buffer); + curl_easy_setopt(curl, CURLOPT_SSL_VERIFYPEER, 0L); + curl_easy_setopt(curl, CURLOPT_SSL_VERIFYHOST, 0L); -class PnpSolverTest : public ::testing::Test { -protected: - void SetUp() override { - // 每个测试前的设置 - } + res = curl_easy_perform(curl); + if (res != CURLE_OK) { + buffer.clear(); + } - void TearDown() override { - // 每个测试后的清理 + curl_easy_cleanup(curl); } -}; - -TEST_F(PnpSolverTest, BasicSmallArmor) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); + curl_global_cleanup(); - // 使用 Eigen::Vector2d 创建 2D 检测点 - // 像素点按 TL,BR,TR,BL 顺序 - const auto eigen_detection = std::array { - Vector2d { 290.0, 220.0 }, // TL - Vector2d { 350.0, 260.0 }, // BR - Vector2d { 350.0, 220.0 }, // TR - Vector2d { 290.0, 260.0 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.x, 0.0) // - << "Translation x should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; - - const auto trans_norm = std::sqrt(solution.result.translation.x * solution.result.translation.x - + solution.result.translation.y * solution.result.translation.y - + solution.result.translation.z * solution.result.translation.z); - - EXPECT_GT(trans_norm, 0.0) << "Translation should not be zero"; + return buffer; } +// --- 核心辅助函数：相机/装甲板参数 --- +// 辅助函数：创建测试用的相机内参 +PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, + double fy = 1.724876404292754e+03, double cx = 7.013056440882832e+02, + double cy = 5.645821718351237e+02, double k1 = -0.064232403853946, + double k2 = -0.087667493884102, double k3 = 0.792381808294582) { -TEST_F(PnpSolverTest, BigArmor) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_big_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 260.0, 200.0 }, // TL - Vector2d { 380.0, 280.0 }, // BR - Vector2d { 380.0, 200.0 }, // TR - Vector2d { 260.0, 280.0 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.x, 0.0) // - << "Translation x should be positive (in front of camera)"; + auto distort_coeff = std::array { k1, k2, 0, 0, k3 }; + PnpSolution::Input input; + input.camera_matrix = { { + { fx, 0.0, cx }, + { 0.0, fy, cy }, + { 0.0, 0.0, 1.0 }, + } }; + input.distort_coeff = distort_coeff; - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; + return input; } -TEST_F(PnpSolverTest, DifferentDistances) { - constexpr double FOCAL_LENGTH = 800.0; - constexpr double ARMOR_WIDTH = 0.135; - - const auto test_distances = std::vector { 1.0, 2.0, 3.0, 5.0 }; - - for (const auto expected_distance : test_distances) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(FOCAL_LENGTH); - solution.input.armor_shape = create_small_armor_shape(); - - // 根据距离计算图像中的大小 - const auto pixel_size = (FOCAL_LENGTH * ARMOR_WIDTH) / expected_distance; - const auto center = Vector2d { 320.0, 240.0 }; - - const auto eigen_detection = std::array { - center + Vector2d { -pixel_size / 2, -pixel_size * 0.2 }, // TL - center + Vector2d { pixel_size / 2, pixel_size * 0.2 }, // BR - center + Vector2d { pixel_size / 2, -pixel_size * 0.2 }, // TR - center + Vector2d { -pixel_size / 2, pixel_size * 0.2 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - const auto distance_error = std::abs(solution.result.translation.x - expected_distance); - - EXPECT_LT(distance_error, expected_distance * 0.05) //误差5% - << "Distance error too large for expected distance " << expected_distance; +// 辅助函数：使用 OpenCV 坐标系定义的装甲板 3D 点 +// [Top Left, Top Right, Bottom Right, Bottom Left] +std::array create_small_armor_shape() { return rmcs::kSmallArmorShapeOpenCV; } + +std::array infer_armor_detection_from_url(std::string_view image_url) { + using namespace rmcs::identifier; + + constexpr auto config_yaml = R"( + model_location: "assets/yolov5.xml" + infer_device: "AUTO" + use_roi_segment: false + use_corner_correction: false + roi_rows: 640 + roi_cols: 640 + input_rows: 640 + input_cols: 640 + min_confidence: 0.8 + score_threshold: 0.7 + nms_threshold: 0.3 + )"; + + auto net = OpenVinoNet {}; + auto yaml = YAML::Load(config_yaml); + + const auto location = std::filesystem::path { __FILE__ }.parent_path(); + auto model_location = location / "../models/yolov5.xml"; + yaml["model_location"] = model_location.string(); + + auto cfg_result = net.configure(yaml); + if (!cfg_result.has_value()) { + throw std::runtime_error("Failed to configure OpenVinoNet: " + cfg_result.error()); + } - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; + const auto buffer = download_image_to_buffer(std::string { image_url }); + if (buffer.empty()) { + throw std::runtime_error("Failed to download image from url: " + std::string { image_url }); + } + auto cv_mat = cv::imdecode(buffer, cv::IMREAD_COLOR); + if (cv_mat.empty()) { + throw std::runtime_error( + "Failed to decode image from buffer for url: " + std::string { image_url }); } -} -TEST_F(PnpSolverTest, EdgeCaseSmallDetection) { - auto solution = PnpSolution {}; + auto image = rmcs::Image {}; + image.details().mat = cv_mat; - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); + auto infer_result = net.sync_infer(image); + if (!infer_result.has_value()) { + throw std::runtime_error("OpenVino inference failed: " + infer_result.error()); + } + const auto& armors = infer_result.value(); + if (armors.empty()) { + throw std::runtime_error("No armor detected from image: " + std::string { image_url }); + } - // 使用 Eigen::Vector2d 创建 2D 检测点（小检测框，远距离） - const auto eigen_detection = std::array { - Vector2d { 318.0, 238.0 }, // TL - Vector2d { 322.0, 242.0 }, // BR - Vector2d { 322.0, 238.0 }, // TR - Vector2d { 318.0, 242.0 }, // BL + const auto& armor = armors.front(); + // [Top Left, Top Right, Bottom Right, Bottom Left] 与 3D 坐标定义一致 + return std::array { + Point2d { armor.tl() }, // 0 + Point2d { armor.tr() }, // 1 + Point2d { armor.br() }, // 2 + Point2d { armor.bl() }, // 3 }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.x, 0.0) // - << "Translation x should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; } -TEST_F(PnpSolverTest, WithDistortion) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(800.0, 320.0, 240.0, { 0.1, -0.2, 0.0, 0.0, 0.0 }); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 290.0, 220.0 }, // TL - Vector2d { 350.0, 260.0 }, // BR - Vector2d { 350.0, 220.0 }, // TR - Vector2d { 290.0, 260.0 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - EXPECT_NO_THROW(solution.solve()); - - // 验证结果 - EXPECT_GT(solution.result.translation.x, 0.0) // - << "Translation x should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; +// 缓存推理结果，避免重复下载和推理 +std::array cached_detection(std::string_view url) { + static std::unordered_map> cache; + if (auto it = cache.find(std::string { url }); it != cache.end()) { + return it->second; + } + auto det = infer_armor_detection_from_url(url); + cache.emplace(url, det); + return det; } -TEST_F(PnpSolverTest, QuaternionValidity) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 290.0, 220.0 }, // TL - Vector2d { 350.0, 260.0 }, // BR - Vector2d { 350.0, 220.0 }, // TR - Vector2d { 290.0, 260.0 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - solution.solve(); +// --- PnP 结果处理辅助函数 --- - // 验证四元数的有效性 - const auto& q = solution.result.orientation; +/** + * @brief 将弧度值规范化到 [-PI/2, PI/2] 范围内 (对应于 [-90度, 90度])。 + * @return 规范化后的角度，范围在 [-PI/2, PI/2] 之间 (弧度)。 + */ +double normalize_angle_90(double angle_rad) { + constexpr double PI = M_PI; + constexpr double HALF_PI = PI / 2.0; - EXPECT_TRUE(std::isfinite(q.x)) << "Quaternion x should be finite"; - EXPECT_TRUE(std::isfinite(q.y)) << "Quaternion y should be finite"; - EXPECT_TRUE(std::isfinite(q.z)) << "Quaternion z should be finite"; - EXPECT_TRUE(std::isfinite(q.w)) << "Quaternion w should be finite"; + // 1. 将角度限制在 [-PI, PI] 范围内 + double normalized = std::fmod(angle_rad + PI, 2.0 * PI); + if (normalized < 0) { + normalized += 2.0 * PI; + } + normalized -= PI; - const auto norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + // 2. 将角度从 [-PI, PI] 映射到 [-PI/2, PI/2] (180度对称处理) + if (normalized > HALF_PI) { + normalized = PI - normalized; + } else if (normalized < -HALF_PI) { + normalized = -PI - normalized; + } - EXPECT_NEAR(norm, 1.0, 0.01) << "Quaternion should be normalized"; + return std::clamp(normalized, -HALF_PI, HALF_PI); } -TEST_F(PnpSolverTest, TranslationValidity) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 290.0, 220.0 }, // TL - Vector2d { 350.0, 260.0 }, // BR - Vector2d { 350.0, 220.0 }, // TR - Vector2d { 290.0, 260.0 }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - // 执行求解 - solution.solve(); - - // 验证平移向量的有效性 - const auto& t = solution.result.translation; - - EXPECT_TRUE(std::isfinite(t.x)) << "Translation x should be finite"; - EXPECT_TRUE(std::isfinite(t.y)) << "Translation y should be finite"; - EXPECT_TRUE(std::isfinite(t.z)) << "Translation z should be finite"; - - EXPECT_GT(t.x, 0.0) << "Translation x should be positive"; +// 四元数转 ZYX 欧拉角 (Yaw, Pitch, Roll)，单位：弧度 +static Eigen::Vector3d quaternion_to_euler_rad(const Orientation& q) { + Quaterniond quat(q.w, q.x, q.y, q.z); // Eigen 构造函数期望 (w,x,y,z) + const auto euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); // yaw(Z), pitch(Y), roll(X) + return { euler[0], euler[1], euler[2] }; } -TEST_F(PnpSolverTest, DifferentFocalLengths) { - const auto focal_lengths = std::vector { 400.0, 800.0, 1200.0, 1600.0 }; - - for (const auto focal : focal_lengths) { - auto solution = PnpSolution {}; +// --- 参数化测试类 --- +class PnpSolverParameterizedTest : public ::testing::TestWithParam { +protected: + PnpSolution solution; + PnpTestCase test_case; + double actual_distance; + double folded_yaw_deg; + double distance_error; + double yaw_error; + const double max_allowed_distance_error_ratio = 0.08; // 8% + const double max_allowed_yaw_error_deg = 15.0; // 15 degrees - solution.input = create_test_input(focal); + void SetUp() override { + test_case = GetParam(); + solution.input = create_test_input(); solution.input.armor_shape = create_small_armor_shape(); - const auto scale = focal / 800.0; - const auto center = Vector2d { 320.0, 240.0 }; - const auto offset = Vector2d { 30.0, 20.0 }; - - const auto eigen_detection = std::array { - center + Vector2d { -offset.x() * scale, -offset.y() * scale }, // TL - center + Vector2d { offset.x() * scale, offset.y() * scale }, // BR - center + Vector2d { offset.x() * scale, -offset.y() * scale }, // TR - center + Vector2d { -offset.x() * scale, offset.y() * scale }, // BL - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); + const auto detection = cached_detection(test_case.url); + solution.input.armor_detection = detection; - // 执行求解 - EXPECT_NO_THROW(solution.solve()); + std::cerr << std::fixed << std::setprecision(1); + std::cerr << "[2D_POINTS] | URL: " << test_case.url << " | TL(" << detection[0].x << "," + << detection[0].y << ")" + << " TR(" << detection[1].x << "," << detection[1].y << ")" + << " BR(" << detection[2].x << "," << detection[2].y << ")" + << " BL(" << detection[3].x << "," << detection[3].y << ")" << std::endl; + std::cerr << std::defaultfloat; // 恢复默认浮点格式 - // 验证结果 - EXPECT_GT(solution.result.translation.x, 0.0) // - << "Translation x should be positive (in front of camera)"; - - EXPECT_TRUE(is_quaternion_normalized(solution.result.orientation)) // - << "Quaternion should be normalized"; - } -} - -TEST_F(PnpSolverTest, Consistency) { - auto solution1 = PnpSolution {}; - auto solution2 = PnpSolution {}; - - solution1.input = create_test_input(); - solution1.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 290.0, 220.0 }, // TL - Vector2d { 350.0, 260.0 }, // BR - Vector2d { 350.0, 220.0 }, // TR - Vector2d { 290.0, 260.0 }, // BL - }; + SCOPED_TRACE(test_case.url); - solution1.input.armor_detection = create_armor_detection(eigen_detection); - solution2.input = solution1.input; // 相同输入 + EXPECT_NO_THROW(solution.solve()) << "Pnp solve failed for URL: " << test_case.url; - // 执行求解 - solution1.solve(); - solution2.solve(); + // --- 解算结果处理 --- + const auto& result = solution.result; + actual_distance = result.translation.x; + distance_error = std::abs(actual_distance - test_case.expected_distance_m); - // 验证一致性 - const auto trans_diff = distance(solution1.result.translation, solution2.result.translation); + const auto euler_rad = quaternion_to_euler_rad(result.orientation); + const double actual_yaw_rad = euler_rad[0]; - EXPECT_LT(trans_diff, 0.001) << "Results should be consistent"; - - const auto quat_diff = - std::sqrt(std::pow(solution1.result.orientation.x - solution2.result.orientation.x, 2) - + std::pow(solution1.result.orientation.y - solution2.result.orientation.y, 2) - + std::pow(solution1.result.orientation.z - solution2.result.orientation.z, 2) - + std::pow(solution1.result.orientation.w - solution2.result.orientation.w, 2)); - - EXPECT_TRUE(quat_diff < 0.001 || quat_diff > 1.9) // - << "Quaternions should be consistent"; -} - -TEST_F(PnpSolverTest, Performance) { - auto solution = PnpSolution {}; - - solution.input = create_test_input(); - solution.input.armor_shape = create_small_armor_shape(); - - // 使用 Eigen::Vector2d 创建 2D 检测点 - const auto eigen_detection = std::array { - Vector2d { 350.0, 220.0 }, - Vector2d { 290.0, 260.0 }, - Vector2d { 290.0, 220.0 }, - Vector2d { 350.0, 260.0 }, - }; - - solution.input.armor_detection = create_armor_detection(eigen_detection); - - constexpr int iterations = 1000; - const auto start = std::chrono::high_resolution_clock::now(); + const double folded_yaw_rad = normalize_angle_90(actual_yaw_rad); + folded_yaw_deg = folded_yaw_rad * 180.0 / M_PI; + yaw_error = std::abs(folded_yaw_deg - test_case.expected_angle_deg); + } - for (int i = 0; i < iterations; i++) { - solution.solve(); + // 打印结构化报告 + void PrintStructuredReport() const { + std::cerr << std::fixed << std::setprecision(4); + std::cerr << "[TEST_REPORT] |" << std::left << std::setw(50) << test_case.url << "|"; + + // 距离信息 + std::cerr << " DISTANCE: " << std::setw(6) << actual_distance << "m (Exp: " << std::setw(4) + << test_case.expected_distance_m << "m) | Error: " << std::setw(6) + << distance_error << "m | Status: "; + if (distance_error < test_case.expected_distance_m * max_allowed_distance_error_ratio) { + std::cerr << "PASS |"; + } else { + std::cerr << "FAIL |"; + } + + // 角度信息 + std::cerr << " YAW: " << std::setw(6) << folded_yaw_deg << "deg (Exp: " << std::setw(4) + << test_case.expected_angle_deg << "deg) | Error: " << std::setw(6) << yaw_error + << "deg | Status: "; + if (yaw_error < max_allowed_yaw_error_deg) { + std::cerr << "PASS |" << std::endl; + } else { + std::cerr << "FAIL |" << std::endl; + } } +}; - const auto end = std::chrono::high_resolution_clock::now(); - const auto duration = std::chrono::duration_cast(end - start); +// 核心测试：距离和角度精度 +TEST_P(PnpSolverParameterizedTest, DistanceAndAngleAccuracy) { + this->PrintStructuredReport(); - const auto avg_time_us = static_cast(duration.count()) / iterations; + // 1. 距离断言 (8% 误差) + const double max_dist_error = + GetParam().expected_distance_m * this->max_allowed_distance_error_ratio; - std::cout << "Average solve time: " << avg_time_us << " microseconds\n"; + EXPECT_LT(this->distance_error, max_dist_error) + << "Distance error (" << this->distance_error << "m) exceeded " << max_dist_error + << "m for " << GetParam().url; - EXPECT_LT(avg_time_us, 1000.0) << "Solve should be fast enough"; + // 2. 角度断言 (15度误差) + EXPECT_LT(this->yaw_error, this->max_allowed_yaw_error_deg) + << "Yaw error (" << this->yaw_error << "deg) exceeded " << this->max_allowed_yaw_error_deg + << "deg for " << GetParam().url; } +// 注册参数化测试 +INSTANTIATE_TEST_SUITE_P(AllImageTests, PnpSolverParameterizedTest, + ::testing::ValuesIn(kPnpTestCases), [](const ::testing::TestParamInfo& info) { + std::string name = info.param.url; + size_t pos = name.find_last_of('/'); + if (pos != std::string::npos) { + name = name.substr(pos + 1); + } + std::replace(name.begin(), name.end(), '.', '_'); + std::replace(name.begin(), name.end(), '-', '_'); + return name; + }); + // 主函数 int main(int argc, char** argv) { + std::cout << "\n--- Starting PnP Accuracy Tests ---\n"; + std::cout << "[TEST_REPORT] | URL (Simplified Name) | DISTANCE: " + "Actual (Exp) | Error | Status | YAW: Actual (Exp) | Error | Status |\n"; + ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); + + std::cout << "--- PnP Accuracy Tests Finished ---\n"; + return result; } diff --git a/tool/visualization.cpp b/tool/visualization.cpp index df746d9c..66cf0402 100644 --- a/tool/visualization.cpp +++ b/tool/visualization.cpp @@ -35,15 +35,12 @@ auto main() -> int { .rclcpp = visual, .device = DeviceId::SENTRY, .camp = CampColor::BLUE, - .id = "", + .id = 0, + .name = "visual_test_armor", .tf = "camera_link", }; - auto& name = config.id; - auto index = char { 'a' }; - std::ranges::for_each(armors, [&](auto& armor) { - name = std::string { "sentry/" } + index++; - armor = std::make_unique(config); - }); + std::ranges::for_each( + armors, [&](auto& armor) { armor = std::make_unique(config); }); } auto posture = std::make_unique( // From f7eab6edf5e063aac263eafa950f319afe8b51f5 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 6 Dec 2025 07:22:33 +0800 Subject: [PATCH 11/36] Update utils --- README.md | 12 ++++++++++++ config/config.yaml | 2 +- src/runtime.cpp | 15 +++++++++++++-- src/utility/tf/static_tf.hpp | 5 +---- test/static_tf.cpp | 4 ++++ 5 files changed, 31 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 60d63340..108c61b2 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,18 @@ 本项目以工程化为最终目的，为机器人提供一个测试与工作流完备，配置友好，重构开销小，错误提示拟人的自瞄系统，方便队员的后续维护和持续开发，为迭代提供舒适的代码基础 +## 核心概念 + +依赖隐藏： + +非侵入式： + +提前编写期检查： + +推迟运行时多态： + +自动化与测试： + ## 部署步骤 先确保海康相机的 SDK 正确构建，再保证 `rmcs_exetutor` 正确构建，如果要运行 RMCS 控制系统的话 diff --git a/config/config.yaml b/config/config.yaml index 2827c1f7..c1f9f21c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -66,7 +66,7 @@ pose_estimator: visualization: framerate: 60 - monitor_host: "192.168.167.79" + monitor_host: "127.0.0.1" monitor_port: "5000" stream_type: "RTP_JEPG" show_armors: true diff --git a/src/runtime.cpp b/src/runtime.cpp index 0b821b0e..302727be 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -35,6 +35,7 @@ auto main() -> int { auto framerate = FramerateCounter {}; framerate.set_interval(5s); + framerate.set_interval(5s); /// Runtime /// @@ -109,8 +110,18 @@ auto main() -> int { .timestamp = Clock::now(), }); - if (framerate.tick()) { - rclcpp_node.info("Framerate: {}hz", framerate.fps()); + auto armors_3d = pose_estimator.solve_pnp(armors_2d); + if (!armors_3d.has_value()) { + continue; + } + + // TODO: pose estimator + // TODO: predictor + // TODO: control + + if (visualization.initialized()) { + visualization.send_image(*image); + std::ignore = visualization.visualize_armors(*armors_3d); } } diff --git a/src/utility/tf/static_tf.hpp b/src/utility/tf/static_tf.hpp index a02b406d..f46f2683 100644 --- a/src/utility/tf/static_tf.hpp +++ b/src/utility/tf/static_tf.hpp @@ -94,9 +94,6 @@ struct Joint { using Result = typename FindInTuple::Result; }; - /// Path - /// TODO: - /// /// Function Based /// @@ -127,7 +124,7 @@ struct Joint { static constexpr auto find() noexcept { using Result = typename Find::Result; static_assert(!std::same_as, "没有找到你想要的变换节点"); - return Result { }; + return Result {}; } template diff --git a/test/static_tf.cpp b/test/static_tf.cpp index d95284e1..c2223731 100644 --- a/test/static_tf.cpp +++ b/test/static_tf.cpp @@ -32,6 +32,10 @@ TEST(static_tf, construct) { SentryTf::foreach_df_with_parent( [](auto parent) { std::println("{} -> {}", parent, T::name); }); + using Result = SentryTf::Find<"0.0.0">::Result; + + constexpr auto result = SentryTf::find<"0.0.0">(); + static_assert(SentryTf::name == "0"); static_assert(SentryTf::child_amount > 0); static_assert(SentryTf::total_amount > 0); From 649267e7d6a72949ebc5bd7610da28fac7239cb8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 9 Dec 2025 05:32:59 +0800 Subject: [PATCH 12/36] Update doc and fix init bug of shared memory util --- README.md | 2 +- doc/hikcamera.md | 15 +++++++++++++++ src/component.cpp | 3 +++ src/kernel/control_system.cpp | 3 +++ 4 files changed, 22 insertions(+), 1 deletion(-) create mode 100644 doc/hikcamera.md diff --git a/README.md b/README.md index 108c61b2..f8108395 100644 --- a/README.md +++ b/README.md @@ -157,7 +157,7 @@ ros2 launch rmcs_auto_aim_v2 launch.py [...] [INFO] [...] [visualization]: Sdp has been written to: /tmp/auto_aim.sdp ``` -随后在本机下载 `VLC`，此外，还需要下载插件：`vlc-plugin-live555` 和 `vlc-plugin-ffmpeg` 以支持播放推流 +对于 `VLC`，需要下载插件：`vlc-plugin-live555` 和 `vlc-plugin-ffmpeg` 接下来只需要将 `/tmp/auto_aim.sdp` 文件拷贝到自己电脑上，使用能够打开`SDP`文件的视频播放器打开即可，也可以使用指令： diff --git a/doc/hikcamera.md b/doc/hikcamera.md new file mode 100644 index 00000000..d9861453 --- /dev/null +++ b/doc/hikcamera.md @@ -0,0 +1,15 @@ +# 海康相机故障速查 + +### 未找到设备但是 `lsusb` 能找到 + +一般是用户没有权限导致的，我们需要在**本机**配置 udev 规则，注意不是在 Docker 容器中： + +```sh +# 创建 Rules 文件 +echo "SUBSYSTEM==\"usb\", ATTR{idVendor}==\"2bdf\", ATTR{idProduct}==\"0001\", MODE=\"0666\"" | sudo tee /etc/udev/rules.d/99-hikcamera.rules +# 重新加载 Rules +sudo udevadm control --reload-rules +sudo udevadm trigger +``` + +执行完上述指令后，理论上就能正确查找相机了 \ No newline at end of file diff --git a/src/component.cpp b/src/component.cpp index 5c6d8f93..16f77a21 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -21,6 +21,7 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { +<<<<<<< HEAD using namespace rmcs_description; if (rmcs_tf.ready()) [[likely]] { auto camera_odom = @@ -36,6 +37,8 @@ class AutoAimComponent final : public rmcs_executor::Component { //... } +======= +>>>>>>> 49d27c5 (Update doc and fix init bug of shared memory util) recv_state(); send_state(); } diff --git a/src/kernel/control_system.cpp b/src/kernel/control_system.cpp index 050613b8..4b4a5912 100644 --- a/src/kernel/control_system.cpp +++ b/src/kernel/control_system.cpp @@ -10,6 +10,7 @@ struct ControlSystem::Impl { ControlState control_state {}; +<<<<<<< HEAD Impl() noexcept { if (!shm_recv.open(util::shared_control_state_name)) { util::panic("Failed to open shared control state"); @@ -19,6 +20,8 @@ struct ControlSystem::Impl { } } +======= +>>>>>>> 49d27c5 (Update doc and fix init bug of shared memory util) /// Send template F> From c92ac374e1d6adf85b2164177c6da5e3ed9f3314 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Thu, 11 Dec 2025 03:06:06 +0800 Subject: [PATCH 13/36] Update doc --- doc/hikcamera.md | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 doc/hikcamera.md diff --git a/doc/hikcamera.md b/doc/hikcamera.md deleted file mode 100644 index d9861453..00000000 --- a/doc/hikcamera.md +++ /dev/null @@ -1,15 +0,0 @@ -# 海康相机故障速查 - -### 未找到设备但是 `lsusb` 能找到 - -一般是用户没有权限导致的，我们需要在**本机**配置 udev 规则，注意不是在 Docker 容器中： - -```sh -# 创建 Rules 文件 -echo "SUBSYSTEM==\"usb\", ATTR{idVendor}==\"2bdf\", ATTR{idProduct}==\"0001\", MODE=\"0666\"" | sudo tee /etc/udev/rules.d/99-hikcamera.rules -# 重新加载 Rules -sudo udevadm control --reload-rules -sudo udevadm trigger -``` - -执行完上述指令后，理论上就能正确查找相机了 \ No newline at end of file From 0a5ebb2a3f6691dccfdb1fff9676312c5ae4e314 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 5 Dec 2025 21:26:07 +0800 Subject: [PATCH 14/36] feat(solve_pnp): add OpenCV to ROS camera coordinate system conversion Implement transformation logic to convert camera coordinates from the OpenCV standard (Z forward, X right, Y down) to the ROS standard (X forward, Y left, Z up). - The PnP solver output (relative pose) is now consistently converted and reported in the ROS camera frame. - Add unit tests for the coordinate transformation function. - Integrate simulated dynamic transforms to test and verify the robustness of the coordinate frame transformations during runtime. --- src/component.cpp | 3 - src/utility/math/solve_pnp.cpp | 82 ++++++++++++++ test/convertion.cpp | 201 +++++++++++++++++++++++++++++++++ 3 files changed, 283 insertions(+), 3 deletions(-) create mode 100644 src/utility/math/solve_pnp.cpp create mode 100644 test/convertion.cpp diff --git a/src/component.cpp b/src/component.cpp index 16f77a21..5c6d8f93 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -21,7 +21,6 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { -<<<<<<< HEAD using namespace rmcs_description; if (rmcs_tf.ready()) [[likely]] { auto camera_odom = @@ -37,8 +36,6 @@ class AutoAimComponent final : public rmcs_executor::Component { //... } -======= ->>>>>>> 49d27c5 (Update doc and fix init bug of shared memory util) recv_state(); send_state(); } diff --git a/src/utility/math/solve_pnp.cpp b/src/utility/math/solve_pnp.cpp new file mode 100644 index 00000000..2719c381 --- /dev/null +++ b/src/utility/math/solve_pnp.cpp @@ -0,0 +1,82 @@ +#include "solve_pnp.hpp" +#include "utility/math/conversion.hpp" +#include +#include +#include +#include + +using namespace rmcs::util; + +template +static auto cast_opencv_matrix(std::array& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { 1, cols, mat_type, source.data() }; +} + +template +static auto cast_opencv_matrix(std::array, rows>& source) { + auto mat_type = int {}; + /* */ if constexpr (std::same_as) { + mat_type = CV_64FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32FC1; + } else if constexpr (std::same_as) { + mat_type = CV_32SC1; + } else { + static_assert(false, "Unsupport mat scale type"); + } + + return cv::Mat { rows, cols, mat_type, source[0].data() }; +} + +auto PnpSolution::solve() noexcept -> void { + const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); + const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); + + const auto armor_shape = std::ranges::to(input.armor_shape + | std::views::transform([](const Point3d& point) { return point.make(); })); + + const auto armor_detection = std::ranges::to(input.armor_detection + | std::views::transform([](const Point2d& point) { return point.make(); })); + + auto rota_vec = cv::Vec3d {}; + auto tran_vec = cv::Vec3d {}; + cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, + false, cv::SOLVEPNP_IPPE); + + auto tran_vec_eigen_opencv = Eigen::Vector3d {}; + tran_vec_eigen_opencv.x() = tran_vec[0]; + tran_vec_eigen_opencv.y() = tran_vec[1]; + tran_vec_eigen_opencv.z() = tran_vec[2]; + + auto rotation_opencv = cv::Mat {}; + cv::Rodrigues(rota_vec, rotation_opencv); + + auto rotation_eigen_opencv = Eigen::Matrix3d {}; + rotation_eigen_opencv << // Col Major + rotation_opencv.at(0, 0), // [0,0] + rotation_opencv.at(0, 1), // [0,1] + rotation_opencv.at(0, 2), // [0,2] + rotation_opencv.at(1, 0), // [1,0] + rotation_opencv.at(1, 1), // [1,1] + rotation_opencv.at(1, 2), // [1,2] + rotation_opencv.at(2, 0), // [2,0] + rotation_opencv.at(2, 1), // [2,1] + rotation_opencv.at(2, 2); // [2,2] + + auto [rotation_eigen_ros, tran_vec_eigen_ros] = + rmcs::util::cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + + result.translation = tran_vec_eigen_ros; + result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; +} diff --git a/test/convertion.cpp b/test/convertion.cpp new file mode 100644 index 00000000..c97fb797 --- /dev/null +++ b/test/convertion.cpp @@ -0,0 +1,201 @@ +#include + +#include "utility/math/conversion.hpp" +#include + +using namespace rmcs::util; + +namespace { + +template +void expect_matrix_near(const MatA& a, const MatB& b, double eps = 1e-9) { + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + EXPECT_NEAR(a(r, c), b(r, c), eps); + } + } +} + +} // namespace + +TEST(Conversion, BasisTransformIdentity) { + Eigen::Vector3d ex { 1, 0, 0 }; + Eigen::Vector3d ey { 0, 1, 0 }; + Eigen::Vector3d ez { 0, 0, 1 }; + + auto T = make_basis_transform(ex, ey, ez, ex, ey, ez); + + expect_matrix_near(T, Eigen::Matrix3d::Identity()); +} + +TEST(Conversion, CvOpticalToRosMatrix) { + const auto T = make_cv_optical_to_ros_camera_link(); + + Eigen::Matrix3d expected; + + // clang-format off + expected << + 0 , 0 , 1, + -1, 0 , 0, + 0 , -1, 0; + // clang-format on + + expect_matrix_near(T, expected); +} + +TEST(Conversion, CvToRosTransformRt) { + Eigen::Matrix3d R_cv = Eigen::Matrix3d::Identity(); + Eigen::Vector3d t_cv { 1., 2., 3. }; + + const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); + + Eigen::Matrix3d T; + + // clang-format off + T << + 0 , 0 , 1, + -1, 0 , 0, + 0 , -1, 0; + // clang-format on + + expect_matrix_near(R_ros, T); + + Eigen::Vector3d expected_t = T * t_cv; + EXPECT_DOUBLE_EQ(t_ros.x(), expected_t.x()); + EXPECT_DOUBLE_EQ(t_ros.y(), expected_t.y()); + EXPECT_DOUBLE_EQ(t_ros.z(), expected_t.z()); +} + +TEST(Conversion, BasisTransformRotate90Z) { + // 源系与目标系：目标系绕 Z 轴 +90 度 + Eigen::Vector3d ex { 1, 0, 0 }; + Eigen::Vector3d ey { 0, 1, 0 }; + Eigen::Vector3d ez { 0, 0, 1 }; + + Eigen::Vector3d ex_rot { 0, 1, 0 }; + Eigen::Vector3d ey_rot { -1, 0, 0 }; + Eigen::Vector3d ez_rot { 0, 0, 1 }; + + auto T = make_basis_transform(ex, ey, ez, ex_rot, ey_rot, ez_rot); + + Eigen::Matrix3d expected, actual; + // clang-format off + actual << + 0 , -1, 0, + 1 , 0, 0, + 0 , 0, 1; + // clang-format on + expected = actual.transpose(); + expect_matrix_near(T, expected); +} + +TEST(Conversion, CvToRosWithRotation) { + // 在 cv 系下绕 Z 轴 90 度，转换后应组合到 T 中 + const auto T = make_cv_optical_to_ros_camera_link(); + + Eigen::AngleAxisd aa_cv(M_PI / 2.0, Eigen::Vector3d::UnitZ()); + Eigen::Matrix3d R_cv = aa_cv.toRotationMatrix(); + Eigen::Vector3d t_cv { 0.5, -0.25, 2.0 }; + + const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); + + Eigen::Matrix3d expected_R = T * R_cv; + expect_matrix_near(R_ros, expected_R); + + Eigen::Vector3d expected_t = T * t_cv; + EXPECT_NEAR(t_ros.x(), expected_t.x(), 1e-12); + EXPECT_NEAR(t_ros.y(), expected_t.y(), 1e-12); + EXPECT_NEAR(t_ros.z(), expected_t.z(), 1e-12); +} + +TEST(Conversion, CvToRosWithNonIdentityBasis) { + // 非标准源/目标基的组合测试 + Eigen::Vector3d src_x { 0, 1, 0 }; // x 指向原来的 y + Eigen::Vector3d src_y { -1, 0, 0 }; // y 指向 -x + Eigen::Vector3d src_z { 0, 0, 1 }; + + Eigen::Vector3d dst_x { 0, 0, 1 }; // x -> 原来的 z + Eigen::Vector3d dst_y { 1, 0, 0 }; // y -> 原来的 x + Eigen::Vector3d dst_z { 0, 1, 0 }; // z -> 原来的 y + + auto T = make_basis_transform(src_x, src_y, src_z, dst_x, dst_y, dst_z); + + Eigen::Matrix3d expected; + // 手工计算：dst^T * src + expected << 0, 0, 1, 0, -1, 0, 1, 0, 0; + + expect_matrix_near(T, expected); +} + +TEST(Conversion, BasisVectorMapping) { + const auto T = make_cv_optical_to_ros_camera_link(); + + const Eigen::Vector3d ex_cv { 1, 0, 0 }; + const Eigen::Vector3d ey_cv { 0, 1, 0 }; + const Eigen::Vector3d ez_cv { 0, 0, 1 }; + + auto ex_ros = T * ex_cv; // 应为 (0,-1,0) + auto ey_ros = T * ey_cv; // 应为 (0,0,-1) + auto ez_ros = T * ez_cv; // 应为 (1,0,0) + + EXPECT_NEAR(ex_ros.x(), 0.0, 1e-12); + EXPECT_NEAR(ex_ros.y(), -1.0, 1e-12); + EXPECT_NEAR(ex_ros.z(), 0.0, 1e-12); + + EXPECT_NEAR(ey_ros.x(), 0.0, 1e-12); + EXPECT_NEAR(ey_ros.y(), 0.0, 1e-12); + EXPECT_NEAR(ey_ros.z(), -1.0, 1e-12); + + EXPECT_NEAR(ez_ros.x(), 1.0, 1e-12); + EXPECT_NEAR(ez_ros.y(), 0.0, 1e-12); + EXPECT_NEAR(ez_ros.z(), 0.0, 1e-12); +} + +TEST(Conversion, ArbitraryVectorTransform) { + Eigen::Vector3d v_cv { 1., 2., 3. }; + auto expected = Eigen::Vector3d { 3., -1., -2. }; + + auto [_, t_ros] = cv_optical_to_ros_camera_link(Eigen::Matrix3d::Identity(), v_cv); + EXPECT_NEAR(t_ros.x(), expected.x(), 1e-12); + EXPECT_NEAR(t_ros.y(), expected.y(), 1e-12); + EXPECT_NEAR(t_ros.z(), expected.z(), 1e-12); + + // TODO:manual calulate; +} + +TEST(Conversion, InverseTransform) { + const auto T = make_cv_optical_to_ros_camera_link(); + Eigen::Matrix3d T_inverse = T.inverse(); + + // 1. 验证逆矩阵是否等于转置 (验证纯旋转特性) + expect_matrix_near(T_inverse, T.transpose()); + + // 2. 验证 T 乘以 T^-1 是否等于单位矩阵 + Eigen::Matrix3d Identity = T * T_inverse; + expect_matrix_near(Identity, Eigen::Matrix3d::Identity()); +} + +TEST(Conversion, CompositeRotation) { + const auto T = make_cv_optical_to_ros_camera_link(); + + // R1: 绕 cv-x 轴 45° + Eigen::AngleAxisd aa_x(M_PI / 4.0, Eigen::Vector3d::UnitX()); + Eigen::Matrix3d R1 = aa_x.toRotationMatrix(); + + // R2: 绕 cv-y 轴 30° + Eigen::AngleAxisd aa_y(M_PI / 6.0, Eigen::Vector3d::UnitY()); + Eigen::Matrix3d R2 = aa_y.toRotationMatrix(); + + // 1. 源系 (CV) 复合旋转 + Eigen::Matrix3d R_cv_comp = R2 * R1; + + // 2. 期望的目标系复合结果 (T * (R2 * R1)) + Eigen::Matrix3d R_ros_expected = T * R_cv_comp; + + // 3. 实际计算 + auto [R_ros_actual, t_ros_acturl] = + cv_optical_to_ros_camera_link(R_cv_comp, Eigen::Vector3d::Zero()); + + // 验证复合结果是否正确 + expect_matrix_near(R_ros_actual, R_ros_expected); +} From fb05f24f0adcf8e6fc9a1da29443852ff8f62852 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Mon, 8 Dec 2025 04:29:32 +0800 Subject: [PATCH 15/36] feat(aim): implement inter-system communication and PnP pipeline visualization Successfully established communication link between the control system and the aiming module. This enables data exchange for target tracking and control. The full PnP (Perspective-n-Point) solving pipeline has been integrated and is running. The resulting pose (translation and rotation) is now visualized in the debugging interface to assist in development. Note: The output coordinate frame for the PnP solution is currently uncalibrated and requires further refinement/tuning to align with the world coordinate system. --- config/config.yaml | 3 +- src/kernel/control_system.cpp | 12 ---- src/kernel/pose_estimator.hpp | 4 ++ .../pose_estimator/pnp_solution.cpp} | 66 ++++++++----------- src/module/pose_estimator/pnp_solution.hpp | 39 +++++++++++ src/runtime.cpp | 13 +++- 6 files changed, 81 insertions(+), 56 deletions(-) rename src/{utility/math/solve_pnp.cpp => module/pose_estimator/pnp_solution.cpp} (57%) create mode 100644 src/module/pose_estimator/pnp_solution.hpp diff --git a/config/config.yaml b/config/config.yaml index c1f9f21c..dc595c9a 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,6 @@ use_visualization: true -use_painted_image: true +use_painted_image: false +open_solved_pnp_visualization: true capturer: show_loss_framerate: false diff --git a/src/kernel/control_system.cpp b/src/kernel/control_system.cpp index 4b4a5912..3b5b4aae 100644 --- a/src/kernel/control_system.cpp +++ b/src/kernel/control_system.cpp @@ -10,18 +10,6 @@ struct ControlSystem::Impl { ControlState control_state {}; -<<<<<<< HEAD - Impl() noexcept { - if (!shm_recv.open(util::shared_control_state_name)) { - util::panic("Failed to open shared control state"); - } - if (!shm_send.open(util::shared_autoaim_state_name)) { - util::panic("Failed to open shared autoaim state"); - } - } - -======= ->>>>>>> 49d27c5 (Update doc and fix init bug of shared memory util) /// Send template F> diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 37f62b74..64cd13a8 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -17,6 +17,10 @@ class PoseEstimator { auto initialize(const YAML::Node&) noexcept -> std::expected; + auto solve_pnp(std::optional> const&) const noexcept -> void; + + auto visualize(RclcppNode& visual_node) -> void; + auto solve_pnp(std::optional> const&) const noexcept -> std::optional>; auto update_imu_link(const Orientation&) noexcept -> void; diff --git a/src/utility/math/solve_pnp.cpp b/src/module/pose_estimator/pnp_solution.cpp similarity index 57% rename from src/utility/math/solve_pnp.cpp rename to src/module/pose_estimator/pnp_solution.cpp index 2719c381..fc190963 100644 --- a/src/utility/math/solve_pnp.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -1,44 +1,11 @@ -#include "solve_pnp.hpp" -#include "utility/math/conversion.hpp" -#include -#include -#include -#include +#include "pnp_solution.hpp" -using namespace rmcs::util; - -template -static auto cast_opencv_matrix(std::array& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } - - return cv::Mat { 1, cols, mat_type, source.data() }; -} - -template -static auto cast_opencv_matrix(std::array, rows>& source) { - auto mat_type = int {}; - /* */ if constexpr (std::same_as) { - mat_type = CV_64FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32FC1; - } else if constexpr (std::same_as) { - mat_type = CV_32SC1; - } else { - static_assert(false, "Unsupport mat scale type"); - } +#include - return cv::Mat { rows, cols, mat_type, source[0].data() }; -} +#include "utility/math/conversion.hpp" +#include "utility/math/solve_pnp.hpp" +using namespace rmcs::util; auto PnpSolution::solve() noexcept -> void { const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); @@ -63,7 +30,7 @@ auto PnpSolution::solve() noexcept -> void { cv::Rodrigues(rota_vec, rotation_opencv); auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Col Major + rotation_eigen_opencv << // Row Major rotation_opencv.at(0, 0), // [0,0] rotation_opencv.at(0, 1), // [0,1] rotation_opencv.at(0, 2), // [0,2] @@ -75,8 +42,27 @@ auto PnpSolution::solve() noexcept -> void { rotation_opencv.at(2, 2); // [2,2] auto [rotation_eigen_ros, tran_vec_eigen_ros] = - rmcs::util::cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + result.genre = input.genre; + result.color = input.color; result.translation = tran_vec_eigen_ros; result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; } + +auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { + using namespace visual; + if (!visualized_armor) { + auto const config = Armor::Config { + + .rclcpp = visual_node, + .device = result.genre, + .camp = result.color, + .id = "solved_pnp_armor", + .tf = "camera_link", + }; + visualized_armor = std::make_unique(config); + } + visualized_armor->impl_move(result.translation, result.orientation); + visualized_armor->update(); +} diff --git a/src/module/pose_estimator/pnp_solution.hpp b/src/module/pose_estimator/pnp_solution.hpp new file mode 100644 index 00000000..05055bff --- /dev/null +++ b/src/module/pose_estimator/pnp_solution.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "utility/math/linear.hpp" +#include "utility/math/point.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/rclcpp/visual/armor.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::util { + +struct PnpSolution { + struct Input { + // Row Major + std::array, 3> camera_matrix; + std::array distort_coeff; + std::array armor_shape; + std::array armor_detection; + DeviceId genre; + CampColor color; + } input; + struct Result { + Translation translation; + Orientation orientation; + DeviceId genre; + CampColor color; + } result; + + std::unique_ptr visualized_armor; + + PnpSolution() noexcept = default; + + auto solve() noexcept -> void; + + auto visualize(RclcppNode&) noexcept -> void; +}; +} diff --git a/src/runtime.cpp b/src/runtime.cpp index 302727be..b32c7a1e 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,6 +6,7 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" +#include "utility/logging/printer.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" @@ -44,13 +45,15 @@ auto main() -> int { auto pose_estimator = kernel::PoseEstimator {}; auto visualization = kernel::Visualization {}; + auto control_system = kernel::ControlSystem {}; auto control_system = kernel::ControlSystem {}; /// Configure /// - auto configuration = util::configuration(); - auto use_visualization = configuration["use_visualization"].as(); - auto use_painted_image = configuration["use_painted_image"].as(); + auto configuration = util::configuration(); + auto use_visualization = configuration["use_visualization"].as(); + auto use_painted_image = configuration["use_painted_image"].as(); + auto open_solved_pnp_visualization = configuration["open_solved_pnp_visualization"].as(); // CAPTURER { @@ -82,6 +85,8 @@ auto main() -> int { handle_result("visualization", result); } + rmcs::Printer printer { "rmcs_auto_aim" }; + for (;;) { if (!util::get_running()) [[unlikely]] break; @@ -106,6 +111,7 @@ auto main() -> int { auto future_state = std::ignore; using namespace rmcs::util; + control_system.update_state({ .timestamp = Clock::now(), }); @@ -130,3 +136,4 @@ auto main() -> int { rclcpp_node.shutdown(); } + From 2eb02cdc9fef0256fdd2a3c47240822ca156c15c Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Mon, 8 Dec 2025 22:50:50 +0800 Subject: [PATCH 16/36] feat(capturer): implement local video stream acquisition Successfully implemented the functionality to acquire and process video streams from local storage or device. --- src/runtime.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/runtime.cpp b/src/runtime.cpp index b32c7a1e..58c6f7c0 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -136,4 +136,3 @@ auto main() -> int { rclcpp_node.shutdown(); } - From 62a067c0f247378fd0b8ab9c55df911075f2775a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 10 Dec 2025 22:17:51 +0800 Subject: [PATCH 17/36] fix(video, pnp): resolve LocalVideo bug and correct PnP point ordering - Corrected a critical bug in the `LocalVideo` module . - Fixed an issue in the `SolvePnp` function where the ordering of 2D image points and their corresponding 3D object points was mismatched, leading to incorrect pose estimation results. The PnP output is now using the correct point correspondence. Further dedicated unit tests for the `SolvePnp` function need to be added in the next step to ensure robustness. --- config/config.yaml | 2 +- src/module/pose_estimator/pnp_solution.cpp | 5 +++-- src/utility/robot/armor.hpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index dc595c9a..94049a36 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,5 +1,5 @@ use_visualization: true -use_painted_image: false +use_painted_image: true open_solved_pnp_visualization: true capturer: diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/module/pose_estimator/pnp_solution.cpp index fc190963..7e1e5282 100644 --- a/src/module/pose_estimator/pnp_solution.cpp +++ b/src/module/pose_estimator/pnp_solution.cpp @@ -30,7 +30,7 @@ auto PnpSolution::solve() noexcept -> void { cv::Rodrigues(rota_vec, rotation_opencv); auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Row Major + rotation_eigen_opencv << // Major rotation_opencv.at(0, 0), // [0,0] rotation_opencv.at(0, 1), // [0,1] rotation_opencv.at(0, 2), // [0,2] @@ -43,11 +43,12 @@ auto PnpSolution::solve() noexcept -> void { auto [rotation_eigen_ros, tran_vec_eigen_ros] = cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); + auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); result.genre = input.genre; result.color = input.color; result.translation = tran_vec_eigen_ros; - result.orientation = Eigen::Quaterniond { rotation_eigen_ros }; + result.orientation = orientation_ros; } auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 4ad1c76e..3f91d8ef 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -88,8 +88,8 @@ constexpr std::array kSmallArmorShapeOpenCV { constexpr std::array kLargeArmorShapeRos { Point3d { 0.0, 0.115, 0.028 }, // Top-left - Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, -0.115, -0.028 }, // Bottom-right + Point3d { 0.0, -0.115, 0.028 }, // Top-right Point3d { 0.0, 0.115, -0.028 } // Bottom-left }; constexpr std::array kSmallArmorShapeRos { From ad3548303462b845c339f94c967fe357b0d62b42 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 13 Dec 2025 07:45:18 +0800 Subject: [PATCH 18/36] refactor(pnp): decouple PnP solver implementation and visualization The PnP (Perspective-n-Point) solver implementation has been separated from its visualization logic. - The `solve_pnp` module now strictly focuses on calculating the 3D pose (rotation and translation) and returns the numerical result. - All drawing, rendering, and coordinate frame visualization code has been moved to a dedicated `visualization` or `debug` module. --- config/config.yaml | 1 - src/kernel/pose_estimator.hpp | 2 - src/module/pose_estimator/pnp_solution.cpp | 69 ---------------------- src/module/pose_estimator/pnp_solution.hpp | 39 ------------ src/runtime.cpp | 9 +-- 5 files changed, 3 insertions(+), 117 deletions(-) delete mode 100644 src/module/pose_estimator/pnp_solution.cpp delete mode 100644 src/module/pose_estimator/pnp_solution.hpp diff --git a/config/config.yaml b/config/config.yaml index 94049a36..c1f9f21c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,6 +1,5 @@ use_visualization: true use_painted_image: true -open_solved_pnp_visualization: true capturer: show_loss_framerate: false diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index 64cd13a8..76f20e49 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -17,8 +17,6 @@ class PoseEstimator { auto initialize(const YAML::Node&) noexcept -> std::expected; - auto solve_pnp(std::optional> const&) const noexcept -> void; - auto visualize(RclcppNode& visual_node) -> void; auto solve_pnp(std::optional> const&) const noexcept diff --git a/src/module/pose_estimator/pnp_solution.cpp b/src/module/pose_estimator/pnp_solution.cpp deleted file mode 100644 index 7e1e5282..00000000 --- a/src/module/pose_estimator/pnp_solution.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "pnp_solution.hpp" - -#include - -#include "utility/math/conversion.hpp" -#include "utility/math/solve_pnp.hpp" - -using namespace rmcs::util; -auto PnpSolution::solve() noexcept -> void { - const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); - const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); - - const auto armor_shape = std::ranges::to(input.armor_shape - | std::views::transform([](const Point3d& point) { return point.make(); })); - - const auto armor_detection = std::ranges::to(input.armor_detection - | std::views::transform([](const Point2d& point) { return point.make(); })); - - auto rota_vec = cv::Vec3d {}; - auto tran_vec = cv::Vec3d {}; - cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, - false, cv::SOLVEPNP_IPPE); - - auto tran_vec_eigen_opencv = Eigen::Vector3d {}; - tran_vec_eigen_opencv.x() = tran_vec[0]; - tran_vec_eigen_opencv.y() = tran_vec[1]; - tran_vec_eigen_opencv.z() = tran_vec[2]; - - auto rotation_opencv = cv::Mat {}; - cv::Rodrigues(rota_vec, rotation_opencv); - - auto rotation_eigen_opencv = Eigen::Matrix3d {}; - rotation_eigen_opencv << // Major - rotation_opencv.at(0, 0), // [0,0] - rotation_opencv.at(0, 1), // [0,1] - rotation_opencv.at(0, 2), // [0,2] - rotation_opencv.at(1, 0), // [1,0] - rotation_opencv.at(1, 1), // [1,1] - rotation_opencv.at(1, 2), // [1,2] - rotation_opencv.at(2, 0), // [2,0] - rotation_opencv.at(2, 1), // [2,1] - rotation_opencv.at(2, 2); // [2,2] - - auto [rotation_eigen_ros, tran_vec_eigen_ros] = - cv_optical_to_ros_camera_link(rotation_eigen_opencv, tran_vec_eigen_opencv); - auto orientation_ros = Eigen::Quaterniond(rotation_eigen_ros).normalized(); - - result.genre = input.genre; - result.color = input.color; - result.translation = tran_vec_eigen_ros; - result.orientation = orientation_ros; -} - -auto PnpSolution::visualize(RclcppNode& visual_node) noexcept -> void { - using namespace visual; - if (!visualized_armor) { - auto const config = Armor::Config { - - .rclcpp = visual_node, - .device = result.genre, - .camp = result.color, - .id = "solved_pnp_armor", - .tf = "camera_link", - }; - visualized_armor = std::make_unique(config); - } - visualized_armor->impl_move(result.translation, result.orientation); - visualized_armor->update(); -} diff --git a/src/module/pose_estimator/pnp_solution.hpp b/src/module/pose_estimator/pnp_solution.hpp deleted file mode 100644 index 05055bff..00000000 --- a/src/module/pose_estimator/pnp_solution.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include - -#include "utility/math/linear.hpp" -#include "utility/math/point.hpp" -#include "utility/rclcpp/node.hpp" -#include "utility/rclcpp/visual/armor.hpp" -#include "utility/robot/color.hpp" -#include "utility/robot/id.hpp" - -namespace rmcs::util { - -struct PnpSolution { - struct Input { - // Row Major - std::array, 3> camera_matrix; - std::array distort_coeff; - std::array armor_shape; - std::array armor_detection; - DeviceId genre; - CampColor color; - } input; - struct Result { - Translation translation; - Orientation orientation; - DeviceId genre; - CampColor color; - } result; - - std::unique_ptr visualized_armor; - - PnpSolution() noexcept = default; - - auto solve() noexcept -> void; - - auto visualize(RclcppNode&) noexcept -> void; -}; -} diff --git a/src/runtime.cpp b/src/runtime.cpp index 58c6f7c0..0cff8505 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -50,10 +50,9 @@ auto main() -> int { /// Configure /// - auto configuration = util::configuration(); - auto use_visualization = configuration["use_visualization"].as(); - auto use_painted_image = configuration["use_painted_image"].as(); - auto open_solved_pnp_visualization = configuration["open_solved_pnp_visualization"].as(); + auto configuration = util::configuration(); + auto use_visualization = configuration["use_visualization"].as(); + auto use_painted_image = configuration["use_painted_image"].as(); // CAPTURER { @@ -85,8 +84,6 @@ auto main() -> int { handle_result("visualization", result); } - rmcs::Printer printer { "rmcs_auto_aim" }; - for (;;) { if (!util::get_running()) [[unlikely]] break; From d336ea9a495d01c9f8d3ba3328bfeec8b5581fa8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 14 Dec 2025 06:48:23 +0800 Subject: [PATCH 19/36] test(pnp): add test file with real-world data and establish performance baseline Implemented a dedicated test file utilizing actual captured data to validate the PnP (Perspective-n-Point) solver's output. The current performance baseline using this real-world data is recorded as: - **Distance Error (within 3m):** 8% - **Angular Error:** 15% --- src/runtime.cpp | 1 - test/convertion.cpp | 201 -------------------------------------------- 2 files changed, 202 deletions(-) delete mode 100644 test/convertion.cpp diff --git a/src/runtime.cpp b/src/runtime.cpp index 0cff8505..389d8982 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,7 +6,6 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" -#include "utility/logging/printer.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" diff --git a/test/convertion.cpp b/test/convertion.cpp deleted file mode 100644 index c97fb797..00000000 --- a/test/convertion.cpp +++ /dev/null @@ -1,201 +0,0 @@ -#include - -#include "utility/math/conversion.hpp" -#include - -using namespace rmcs::util; - -namespace { - -template -void expect_matrix_near(const MatA& a, const MatB& b, double eps = 1e-9) { - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - EXPECT_NEAR(a(r, c), b(r, c), eps); - } - } -} - -} // namespace - -TEST(Conversion, BasisTransformIdentity) { - Eigen::Vector3d ex { 1, 0, 0 }; - Eigen::Vector3d ey { 0, 1, 0 }; - Eigen::Vector3d ez { 0, 0, 1 }; - - auto T = make_basis_transform(ex, ey, ez, ex, ey, ez); - - expect_matrix_near(T, Eigen::Matrix3d::Identity()); -} - -TEST(Conversion, CvOpticalToRosMatrix) { - const auto T = make_cv_optical_to_ros_camera_link(); - - Eigen::Matrix3d expected; - - // clang-format off - expected << - 0 , 0 , 1, - -1, 0 , 0, - 0 , -1, 0; - // clang-format on - - expect_matrix_near(T, expected); -} - -TEST(Conversion, CvToRosTransformRt) { - Eigen::Matrix3d R_cv = Eigen::Matrix3d::Identity(); - Eigen::Vector3d t_cv { 1., 2., 3. }; - - const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); - - Eigen::Matrix3d T; - - // clang-format off - T << - 0 , 0 , 1, - -1, 0 , 0, - 0 , -1, 0; - // clang-format on - - expect_matrix_near(R_ros, T); - - Eigen::Vector3d expected_t = T * t_cv; - EXPECT_DOUBLE_EQ(t_ros.x(), expected_t.x()); - EXPECT_DOUBLE_EQ(t_ros.y(), expected_t.y()); - EXPECT_DOUBLE_EQ(t_ros.z(), expected_t.z()); -} - -TEST(Conversion, BasisTransformRotate90Z) { - // 源系与目标系：目标系绕 Z 轴 +90 度 - Eigen::Vector3d ex { 1, 0, 0 }; - Eigen::Vector3d ey { 0, 1, 0 }; - Eigen::Vector3d ez { 0, 0, 1 }; - - Eigen::Vector3d ex_rot { 0, 1, 0 }; - Eigen::Vector3d ey_rot { -1, 0, 0 }; - Eigen::Vector3d ez_rot { 0, 0, 1 }; - - auto T = make_basis_transform(ex, ey, ez, ex_rot, ey_rot, ez_rot); - - Eigen::Matrix3d expected, actual; - // clang-format off - actual << - 0 , -1, 0, - 1 , 0, 0, - 0 , 0, 1; - // clang-format on - expected = actual.transpose(); - expect_matrix_near(T, expected); -} - -TEST(Conversion, CvToRosWithRotation) { - // 在 cv 系下绕 Z 轴 90 度，转换后应组合到 T 中 - const auto T = make_cv_optical_to_ros_camera_link(); - - Eigen::AngleAxisd aa_cv(M_PI / 2.0, Eigen::Vector3d::UnitZ()); - Eigen::Matrix3d R_cv = aa_cv.toRotationMatrix(); - Eigen::Vector3d t_cv { 0.5, -0.25, 2.0 }; - - const auto [R_ros, t_ros] = cv_optical_to_ros_camera_link(R_cv, t_cv); - - Eigen::Matrix3d expected_R = T * R_cv; - expect_matrix_near(R_ros, expected_R); - - Eigen::Vector3d expected_t = T * t_cv; - EXPECT_NEAR(t_ros.x(), expected_t.x(), 1e-12); - EXPECT_NEAR(t_ros.y(), expected_t.y(), 1e-12); - EXPECT_NEAR(t_ros.z(), expected_t.z(), 1e-12); -} - -TEST(Conversion, CvToRosWithNonIdentityBasis) { - // 非标准源/目标基的组合测试 - Eigen::Vector3d src_x { 0, 1, 0 }; // x 指向原来的 y - Eigen::Vector3d src_y { -1, 0, 0 }; // y 指向 -x - Eigen::Vector3d src_z { 0, 0, 1 }; - - Eigen::Vector3d dst_x { 0, 0, 1 }; // x -> 原来的 z - Eigen::Vector3d dst_y { 1, 0, 0 }; // y -> 原来的 x - Eigen::Vector3d dst_z { 0, 1, 0 }; // z -> 原来的 y - - auto T = make_basis_transform(src_x, src_y, src_z, dst_x, dst_y, dst_z); - - Eigen::Matrix3d expected; - // 手工计算：dst^T * src - expected << 0, 0, 1, 0, -1, 0, 1, 0, 0; - - expect_matrix_near(T, expected); -} - -TEST(Conversion, BasisVectorMapping) { - const auto T = make_cv_optical_to_ros_camera_link(); - - const Eigen::Vector3d ex_cv { 1, 0, 0 }; - const Eigen::Vector3d ey_cv { 0, 1, 0 }; - const Eigen::Vector3d ez_cv { 0, 0, 1 }; - - auto ex_ros = T * ex_cv; // 应为 (0,-1,0) - auto ey_ros = T * ey_cv; // 应为 (0,0,-1) - auto ez_ros = T * ez_cv; // 应为 (1,0,0) - - EXPECT_NEAR(ex_ros.x(), 0.0, 1e-12); - EXPECT_NEAR(ex_ros.y(), -1.0, 1e-12); - EXPECT_NEAR(ex_ros.z(), 0.0, 1e-12); - - EXPECT_NEAR(ey_ros.x(), 0.0, 1e-12); - EXPECT_NEAR(ey_ros.y(), 0.0, 1e-12); - EXPECT_NEAR(ey_ros.z(), -1.0, 1e-12); - - EXPECT_NEAR(ez_ros.x(), 1.0, 1e-12); - EXPECT_NEAR(ez_ros.y(), 0.0, 1e-12); - EXPECT_NEAR(ez_ros.z(), 0.0, 1e-12); -} - -TEST(Conversion, ArbitraryVectorTransform) { - Eigen::Vector3d v_cv { 1., 2., 3. }; - auto expected = Eigen::Vector3d { 3., -1., -2. }; - - auto [_, t_ros] = cv_optical_to_ros_camera_link(Eigen::Matrix3d::Identity(), v_cv); - EXPECT_NEAR(t_ros.x(), expected.x(), 1e-12); - EXPECT_NEAR(t_ros.y(), expected.y(), 1e-12); - EXPECT_NEAR(t_ros.z(), expected.z(), 1e-12); - - // TODO:manual calulate; -} - -TEST(Conversion, InverseTransform) { - const auto T = make_cv_optical_to_ros_camera_link(); - Eigen::Matrix3d T_inverse = T.inverse(); - - // 1. 验证逆矩阵是否等于转置 (验证纯旋转特性) - expect_matrix_near(T_inverse, T.transpose()); - - // 2. 验证 T 乘以 T^-1 是否等于单位矩阵 - Eigen::Matrix3d Identity = T * T_inverse; - expect_matrix_near(Identity, Eigen::Matrix3d::Identity()); -} - -TEST(Conversion, CompositeRotation) { - const auto T = make_cv_optical_to_ros_camera_link(); - - // R1: 绕 cv-x 轴 45° - Eigen::AngleAxisd aa_x(M_PI / 4.0, Eigen::Vector3d::UnitX()); - Eigen::Matrix3d R1 = aa_x.toRotationMatrix(); - - // R2: 绕 cv-y 轴 30° - Eigen::AngleAxisd aa_y(M_PI / 6.0, Eigen::Vector3d::UnitY()); - Eigen::Matrix3d R2 = aa_y.toRotationMatrix(); - - // 1. 源系 (CV) 复合旋转 - Eigen::Matrix3d R_cv_comp = R2 * R1; - - // 2. 期望的目标系复合结果 (T * (R2 * R1)) - Eigen::Matrix3d R_ros_expected = T * R_cv_comp; - - // 3. 实际计算 - auto [R_ros_actual, t_ros_acturl] = - cv_optical_to_ros_camera_link(R_cv_comp, Eigen::Vector3d::Zero()); - - // 验证复合结果是否正确 - expect_matrix_near(R_ros_actual, R_ros_expected); -} From fc3238400ab81da6864ba14ea51bb1b5e16a0e65 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Mon, 15 Dec 2025 19:21:31 +0800 Subject: [PATCH 20/36] chore(deps): synchronize with upstream main branch Pulled latest changes from the main branch to ensure the current feature branch is up-to-date with recent bug fixes and dependencies updates. --- src/runtime.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/runtime.cpp b/src/runtime.cpp index 389d8982..322e9fc7 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -44,7 +44,6 @@ auto main() -> int { auto pose_estimator = kernel::PoseEstimator {}; auto visualization = kernel::Visualization {}; - auto control_system = kernel::ControlSystem {}; auto control_system = kernel::ControlSystem {}; /// Configure @@ -107,7 +106,7 @@ auto main() -> int { auto future_state = std::ignore; using namespace rmcs::util; - + control_system.update_state({ .timestamp = Clock::now(), }); From d81a2085571c68b0ca5f3250e02b7cf122ede61c Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 21:02:36 +0800 Subject: [PATCH 21/36] Update doc/utility.md Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- doc/utility.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/utility.md b/doc/utility.md index d30b1140..1e0d6f5e 100644 --- a/doc/utility.md +++ b/doc/utility.md @@ -53,7 +53,7 @@ - **[math/sigmoid.hpp](../src/utility/math/sigmoid.hpp)**: 提供 sigmoid 函数实现，用于数值稳定计算。 - **[math/solve_armors.hpp](../src/utility/math/solve_armors.hpp)**: 提供装甲板正解和逆解算法。`ArmorsForwardSolution` 用于根据机器人位姿计算四个装甲板的位置和姿态。 - **[math/solve_armors.cpp](../src/utility/math/solve_armors.cpp)**: 正解算法的实现。 -- **[math/solve_pnp.hpp](../src/utility/math/solve_pnp.cpp)**: 提供将相机内外参的转换函数供pnp解算使用。 +- **[math/solve_pnp.hpp](../src/utility/math/solve_pnp.hpp)**: 提供将相机内外参的转换函数供pnp解算使用。 ### ROS2 扩展 (rclcpp 包装器) - **[rclcpp/node.hpp](../src/utility/rclcpp/node.hpp)**: 提供 `RclcppNode` 类，封装 ROS2 节点的基本功能，包括日志、发布主题前缀管理等。**使用 PIMPL 模式完全隐藏 rclcpp 头文件**，推荐在需要避免编译依赖时使用。 From 76c802e6ce332a36972e38d55e784bc666aa9128 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 21:06:57 +0800 Subject: [PATCH 22/36] Update src/kernel/visualization.cpp Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- src/kernel/visualization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 813e542c..4591ccf8 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -128,8 +128,8 @@ struct Visualization::Impl { return session->push_frame(mat); } - auto visualize_armors(std::span const& armors) const -> bool { + if (!is_initialized) return false; return armor_visualizer->visualize(armors); } }; From f5b064df94387db79ea9b77aee8e6ce40a80344a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 21:24:49 +0800 Subject: [PATCH 23/36] fix(pose): remove redundant visual_armors variable --- src/kernel/pose_estimator.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 9e3a56b9..edd60665 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -29,7 +29,6 @@ struct PoseEstimator::Impl { Config config; PnpSolution pnp_solution {}; - std::vector> visual_armors {}; Printer log { "PoseEstimator" }; @@ -61,11 +60,7 @@ struct PoseEstimator::Impl { auto solve_pnp(std::optional> const& armors) noexcept -> std::optional> { if (!armors.has_value()) return std::nullopt; - auto _armors = (*armors); - - auto new_size = _armors.size(); - visual_armors.reserve(new_size); - visual_armors.resize(new_size); + auto const& _armors = *armors; auto armor_shape = [](ArmorShape shape) { if (shape == ArmorShape::SMALL) { From 3b0d011dd8b0cbe67aee364c8b8d74679f40ea2e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 21:57:14 +0800 Subject: [PATCH 24/36] fix(video): modify LocalVideo function signatures for interface compatibility --- src/kernel/pose_estimator.cpp | 1 - src/module/capturer/local_video.cpp | 8 ++------ src/module/capturer/local_video.hpp | 2 +- 3 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index edd60665..3a9a9318 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -4,7 +4,6 @@ #include "utility/logging/printer.hpp" #include "utility/math/solve_pnp.hpp" #include "utility/math/solve_pnp/pnp_solution.hpp" -#include "utility/rclcpp/visual/armor.hpp" #include "utility/serializable.hpp" #include "utility/yaml/tf.hpp" diff --git a/src/module/capturer/local_video.cpp b/src/module/capturer/local_video.cpp index efd640fb..af63632c 100644 --- a/src/module/capturer/local_video.cpp +++ b/src/module/capturer/local_video.cpp @@ -61,13 +61,11 @@ struct LocalVideo::Impl { auto connected() const noexcept -> bool { return capturer.has_value() && capturer->isOpened(); } - auto disconnect() noexcept -> std::expected { + auto disconnect() noexcept -> void { if (capturer.has_value()) { capturer.reset(); } interval_duration = std::chrono::nanoseconds { 0 }; - - return {}; } auto wait_image() noexcept -> std::expected, std::string> { @@ -122,9 +120,7 @@ auto LocalVideo::connect() noexcept -> std::expected { return auto LocalVideo::connected() const noexcept -> bool { return pimpl->connected(); } -auto LocalVideo::disconnect() noexcept -> std::expected { - return pimpl->disconnect(); -} +auto LocalVideo::disconnect() noexcept -> void { return pimpl->disconnect(); } LocalVideo::LocalVideo() noexcept : pimpl { std::make_unique() } { } diff --git a/src/module/capturer/local_video.hpp b/src/module/capturer/local_video.hpp index a0bdaba7..bf7515cf 100644 --- a/src/module/capturer/local_video.hpp +++ b/src/module/capturer/local_video.hpp @@ -37,7 +37,7 @@ struct LocalVideo { auto connected() const noexcept -> bool; - auto disconnect() noexcept -> std::expected; + auto disconnect() noexcept -> void; auto wait_image() noexcept -> std::expected, std::string>; }; From 42004dd5de3597591eb95dcbb4ccaac9283f7fb7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 22:04:35 +0800 Subject: [PATCH 25/36] Update src/module/debug/visualization/armor_visualizer.cpp Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com> --- src/module/debug/visualization/armor_visualizer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index 03fa117c..d98492c7 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -51,6 +51,7 @@ struct ArmorVisualizer::Impl final { shadow.genre = input.genre; shadow.color = input.color; + shadow.id = input.id; } armor_ptr->move(input.translation, input.orientation); From 3f38bfb294b6b75578329482ca1055e46492393d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 22:30:35 +0800 Subject: [PATCH 26/36] fix(visual):reuse publisher creation function and remove redundancy --- src/utility/rclcpp/visual/armor.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index c78b8374..03f71020 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -25,12 +25,10 @@ struct Armor::Impl { static auto create_rclcpp_publisher(Config const& config) -> std::shared_ptr> { - const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.name }; - if (!config.rclcpp.details) { + if (!config.rclcpp.details) util::panic("Rclcpp node details are required in config to create publisher."); - } return config.rclcpp.details->make_pub(topic_name, qos::debug); } @@ -90,8 +88,7 @@ struct Armor::Impl { auto update() noexcept -> void { if (!rclcpp_pub) { - const auto topic_name { config->rclcpp.get_pub_topic_prefix() + config->name }; - rclcpp_pub = config->rclcpp.details->make_pub(topic_name, qos::debug); + rclcpp_pub = create_rclcpp_publisher(*config); } MarkerArray visual_marker; From c7d7d242f0d762969def0506a2b381ab0634d1f2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 23:40:22 +0800 Subject: [PATCH 27/36] chore(deps): synchronize with upstream main branch --- src/runtime.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/runtime.cpp b/src/runtime.cpp index 322e9fc7..486b71ba 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -101,10 +101,6 @@ auto main() -> int { visualization.send_image(*image); } - auto armor_3d = std::ignore; - - auto future_state = std::ignore; - using namespace rmcs::util; control_system.update_state({ From ac813255776395b759d07760b57cd02ae955150f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Dec 2025 23:52:23 +0800 Subject: [PATCH 28/36] chore(cmake): remove redundant module from test/CMakeLists.txt --- test/CMakeLists.txt | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f9c68ad8..22b189ab 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -117,24 +117,6 @@ ament_add_gtest( ${TEST_DIR}/transform_communication.cpp ) -if(NOT DEFINED BUILD_VIS_EXAMPLE OR BUILD_VIS_EXAMPLE) - add_executable( - example_visualization - ${TEST_DIR}/visualization.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/visual/armor.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/visual/posture.cpp - ${RMCS_SRC_DIR}/utility/panic.cpp - ${RMCS_SRC_DIR}/utility/math/solve_armors.cpp - ${RMCS_SRC_DIR}/utility/rclcpp/node.cpp - ) - target_link_libraries( - example_visualization - rclcpp::rclcpp - ${visualization_msgs_LIBRARIES} - ${geometry_msgs_LIBRARIES} - ) -endif() - if(HIKCAMERA_AVAILABLE) # Hikcamera add_executable( From 1f16df6f6d7c1a3f150305cac393b9dc7e34afd5 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 00:09:16 +0800 Subject: [PATCH 29/36] feat(pnp): implement failure handling logic for PnP solver --- src/utility/math/solve_pnp/pnp_solution.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/utility/math/solve_pnp/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp index c52fd82f..2a5ef776 100644 --- a/src/utility/math/solve_pnp/pnp_solution.cpp +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -22,8 +22,10 @@ auto PnpSolution::solve() noexcept -> void { auto rota_vec = cv::Vec3d {}; auto tran_vec = cv::Vec3d {}; - cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, - false, cv::SOLVEPNP_IPPE); + auto success = cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, + rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); + + if (!success) return; auto tran_vec_eigen_opencv = Eigen::Vector3d {}; cv::cv2eigen(tran_vec, tran_vec_eigen_opencv); From 43c44a9cf7bb7c5825e9f724f305a93849da7b69 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 02:50:08 +0800 Subject: [PATCH 30/36] chore(tooling): add resource download script for testing assets --- test/asset.yml | 18 +++++-- test/download_assets.sh | 117 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 132 insertions(+), 3 deletions(-) create mode 100755 test/download_assets.sh diff --git a/test/asset.yml b/test/asset.yml index 3259ccac..02cddef5 100644 --- a/test/asset.yml +++ b/test/asset.yml @@ -1,3 +1,15 @@ -b4_front_2_armors: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg -b1_orientation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/orientation.mp4 -b1_translation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/translation.mp4 +resources: + # 资源ID + b4_front_2_armors: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg + b1_orientation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/orientation.mp4 + b1_translation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/translation.mp4 + + #pnp + blue_0_5m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m.jpg" + blue_0_5m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m-45degree.jpg" + blue_1_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m.jpg" + blue_1_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m-45degree.jpg" + blue_2_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m.jpg" + blue_2_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m-45degree.jpg" + blue_3_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m.jpg" + blue_3_0m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m-45degree.jpg" diff --git a/test/download_assets.sh b/test/download_assets.sh new file mode 100755 index 00000000..59b5c4e5 --- /dev/null +++ b/test/download_assets.sh @@ -0,0 +1,117 @@ +#!/bin/bash + +# 设置：如果任何命令失败，立即退出，对管道也生效 +set -eo pipefail + +# --- 全局变量 --- +DOWNLOAD_DIR="/tmp/auto_aim" +# 初始化为空字符串，表示当前没有文件在下载** +CURRENT_DOWNLOAD_PATH="" +# ------------------ + +# --- 配置 --- +# 1. 获取脚本自身的目录，用于定位配置文件 +SCRIPT_DIR=$(cd "$(dirname "$0")" && pwd) +CONFIG_YML="$SCRIPT_DIR/asset.yml" +# ----------------- + +# --- 信号处理函数 --- +cleanup() { + echo "" + echo "[INTERRUPT] 捕获到 Ctrl+C。开始清理..." >&2 + + if [ -n "$CURRENT_DOWNLOAD_PATH" ]; then + if [ -f "$CURRENT_DOWNLOAD_PATH" ]; then + echo "[CLEANUP] 删除不完整的文件: $CURRENT_DOWNLOAD_PATH" >&2 + rm -f "$CURRENT_DOWNLOAD_PATH" + fi + fi + echo "[EXIT] 脚本已终止。" >&2 + exit 1 +} + +trap cleanup INT TERM + +check_dependencies() { + if ! command -v yq &> /dev/null || ! command -v curl &> /dev/null; then + echo "[FATAL] 错误: 缺少必要的依赖工具 (yq 或 curl)。" >&2 + echo "请安装 yq (用于解析 YAML) 和 curl (用于下载)。" >&2 + exit 1 + fi +} + +download_asset() { + local asset_id="$1" + local url="$2" + local local_path="$DOWNLOAD_DIR/$asset_id" + + if [ -f "$local_path" ]; then + echo " [SKIP] $asset_id 已存在。" + return 0 + fi + + echo " [INFO] 正在下载 $asset_id..." + + CURRENT_DOWNLOAD_PATH="$local_path" + + if curl -fsSL -o "$local_path" "$url" &> /dev/null; then + echo " [SUCCESS] -> $local_path" + CURRENT_DOWNLOAD_PATH="" + return 0 + else + echo " [ERROR] 下载失败: $asset_id (URL: $url)" >&2 + rm -f "$local_path" + CURRENT_DOWNLOAD_PATH="" + return 1 + fi +} + +main() { + local RESOURCE_MAPPING + local FAILED_COUNT=0 + + check_dependencies + + echo "--- 资源下载脚本启动 ---" + + if [ ! -f "$CONFIG_YML" ]; then + echo "[FATAL] 配置文件未找到于 $CONFIG_YML" >&2 + trap - INT TERM + exit 1 + fi + + mkdir -p "$DOWNLOAD_DIR" + + RESOURCE_MAPPING=$(yq '.resources | to_entries | .[] | "\(.key)|\(.value)"' "$CONFIG_YML") + + if [ -z "$RESOURCE_MAPPING" ]; then + echo "[WARNING] YAML 文件中未定义任何 'resources'，退出。" + trap - INT TERM + exit 0 + fi + + echo "找到 $(echo "$RESOURCE_MAPPING" | wc -l | tr -d '[:space:]') 个资源需要处理。" + + while IFS='|' read -r asset_id url; do + asset_id=$(echo "$asset_id" | tr -d '"') + url=$(echo "$url" | tr -d '"') + + download_asset "$asset_id" "$url" || FAILED_COUNT=$((FAILED_COUNT + 1)) + + done <<< "$RESOURCE_MAPPING" + + echo "" + echo "--- 资源下载完成 ---" + + trap - INT TERM + + if [ "$FAILED_COUNT" -eq 0 ]; then + echo "[INFO] 所有资源下载成功或已跳过。" + exit 0 + else + echo "[ERROR] 警告：有 $FAILED_COUNT 个资源下载失败。" >&2 + exit 1 + fi +} + +main \ No newline at end of file From 1a8ecaf6311ab6ade9281091af06cf3757818b27 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 04:14:49 +0800 Subject: [PATCH 31/36] feat(test): implement YAML configuration for test resources and integrate into CI Implemented a robust configuration system using YAML files to specify all necessary test resources. - **Resource Configuration:** Resources for testing `model_infer` and `solve_pnp` are now loaded dynamically from a centralized YAML configuration file. - **Test Adaptation:** Adapted `model_infer` and `solve_pnp` tests to consume resources based on the new configuration. - **CI Integration:** The new configuration-driven tests have been integrated into the automated testing pipeline (CI/CD process). - **Environment Support:** The configuration supports loading environment variables for sensitive paths or using pre-defined default values, enhancing deployment flexibility. --- .github/workflows/gtest.yml | 11 ++-- test/CMakeLists.txt | 2 - test/asset.yml | 8 +-- test/download_assets.sh | 20 +++--- test/model_infer.cpp | 20 +++++- test/solve_pnp.cpp | 123 +++++++++++++----------------------- 6 files changed, 80 insertions(+), 104 deletions(-) diff --git a/.github/workflows/gtest.yml b/.github/workflows/gtest.yml index d18c4144..f46551f8 100644 --- a/.github/workflows/gtest.yml +++ b/.github/workflows/gtest.yml @@ -16,8 +16,7 @@ jobs: env: WS_DIR: ${{ github.workspace }}/ws SRC_DIR: ${{ github.workspace }}/ws/src - IMAGE_URL: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg - IMAGE_PATH: /tmp/model_infer_example.jpg + TEST_ASSETS_ROOT: /tmp/auto_aim steps: - name: Checkout repository into ROS2 workspace @@ -26,9 +25,11 @@ jobs: repository: Alliance-Algorithm/rmcs_auto_aim_v2 path: ws/src/rmcs_auto_aim_v2 - - name: Download test frame + - name: Download test assets + shell: bash run: | - curl -fsSL "$IMAGE_URL" -o "$IMAGE_PATH" + cd "$SRC_DIR/rmcs_auto_aim_v2/test" + TEST_ASSETS_ROOT="$TEST_ASSETS_ROOT" ./download_assets.sh - name: Configure test project shell: bash @@ -49,4 +50,4 @@ jobs: run: | source /opt/ros/jazzy/setup.bash cd "$SRC_DIR/rmcs_auto_aim_v2/test" - IMAGE="$IMAGE_PATH" ctest --test-dir build --output-on-failure + ctest --test-dir build --output-on-failure diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 22b189ab..008f4ec6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(geometry_msgs REQUIRED) find_package(yaml-cpp REQUIRED) find_package(OpenVINO REQUIRED) find_package(OpenCV 4.5 REQUIRED) -find_package(CURL REQUIRED) include_directories( ${RMCS_SRC_DIR} @@ -97,7 +96,6 @@ target_link_libraries( ${OpenCV_LIBRARIES} openvino::runtime yaml-cpp::yaml-cpp - CURL::libcurl ) # Static TF diff --git a/test/asset.yml b/test/asset.yml index 02cddef5..c7e5578c 100644 --- a/test/asset.yml +++ b/test/asset.yml @@ -1,10 +1,8 @@ resources: - # 资源ID - b4_front_2_armors: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg - b1_orientation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/orientation.mp4 - b1_translation: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/translation.mp4 + #model_infer + model_infer_img: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg - #pnp + #solve_pnp blue_0_5m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m.jpg" blue_0_5m_45deg: "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m-45degree.jpg" blue_1_0m_0deg: "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m.jpg" diff --git a/test/download_assets.sh b/test/download_assets.sh index 59b5c4e5..ab12af2a 100755 --- a/test/download_assets.sh +++ b/test/download_assets.sh @@ -3,16 +3,14 @@ # 设置：如果任何命令失败，立即退出，对管道也生效 set -eo pipefail -# --- 全局变量 --- -DOWNLOAD_DIR="/tmp/auto_aim" -# 初始化为空字符串，表示当前没有文件在下载** -CURRENT_DOWNLOAD_PATH="" -# ------------------ - # --- 配置 --- -# 1. 获取脚本自身的目录，用于定位配置文件 +# 获取脚本自身的目录，用于定位配置文件 SCRIPT_DIR=$(cd "$(dirname "$0")" && pwd) CONFIG_YML="$SCRIPT_DIR/asset.yml" +# 资源存放目录，优先使用 TEST_ASSETS_ROOT，默认为 /tmp/auto_aim（与测试代码一致） +DOWNLOAD_DIR="${TEST_ASSETS_ROOT:-/tmp/auto_aim}" +# 初始化为空字符串，表示当前没有文件在下载** +CURRENT_DOWNLOAD_PATH="" # ----------------- # --- 信号处理函数 --- @@ -43,14 +41,16 @@ check_dependencies() { download_asset() { local asset_id="$1" local url="$2" - local local_path="$DOWNLOAD_DIR/$asset_id" + local filename + filename=$(basename "$url") + local local_path="$DOWNLOAD_DIR/$filename" if [ -f "$local_path" ]; then echo " [SKIP] $asset_id 已存在。" return 0 fi - echo " [INFO] 正在下载 $asset_id..." + echo " [INFO] 正在下载 $asset_id -> $filename..." CURRENT_DOWNLOAD_PATH="$local_path" @@ -114,4 +114,4 @@ main() { fi } -main \ No newline at end of file +main diff --git a/test/model_infer.cpp b/test/model_infer.cpp index cca80c9b..11a53927 100644 --- a/test/model_infer.cpp +++ b/test/model_infer.cpp @@ -29,6 +29,21 @@ constexpr auto config = R"( nms_threshold: 0.3 )"; +// --- 资源路径 --- +static std::filesystem::path asset_root() { + if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { + return std::filesystem::path { env }; + } + + const char* default_path = "/tmp/auto_aim"; + + return std::filesystem::path { default_path }; +} + +static std::filesystem::path asset_path(std::string_view filename) { + return asset_root() / filename; +} + TEST(model, sync_infer) { using namespace rmcs::identifier; @@ -41,13 +56,12 @@ TEST(model, sync_infer) { auto result = net.configure(yaml); ASSERT_TRUE(result.has_value()) << error_head << result.error(); - auto image_location = std::getenv("IMAGE"); - ASSERT_NE(image_location, nullptr) << error_head << "Set env 'IMAGE' to pass infer source"; + const auto image_location = asset_path("model_infer_example.jpg"); auto image { Image {} }; image.details().mat = cv::imread(image_location); ASSERT_FALSE(image.details().mat.empty()) - << error_head << std::format("Failed to read image from '{}'", image_location); + << error_head << "Failed to read image from " + std::string(image_location); const auto use_roi_segment = yaml["use_roi_segment"].as(); const auto roi_cols = yaml["roi_cols"].as(); diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index 949b19d8..22e6ccc7 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -1,12 +1,13 @@ #include // for std::clamp, std::replace +#include // for std::getenv #include #include #include // for std::setprecision #include // for structured output -#include +#include +#include #include -#include #include #include #include @@ -23,59 +24,41 @@ using Eigen::Quaterniond; using Eigen::Vector2d; using Eigen::Vector3d; +// --- 资源路径 --- +static std::filesystem::path asset_root() { + if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { + return std::filesystem::path { env }; + } + + const char* default_path = "/tmp/auto_aim"; + + return std::filesystem::path { default_path }; +} + +static std::filesystem::path asset_path(std::string_view filename) { + return asset_root() / filename; +} + // --- 测试数据 --- struct PnpTestCase { - std::string url; + std::string filename; double expected_distance_m; // 预期的目标距离（米） double expected_angle_deg; // 预期的偏航角（度，0 或 45） }; -// 所有 URL 测试数据 +// 所有本地测试数据 const std::vector kPnpTestCases = { - { "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m.jpg", 0.5, 0.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-0.5m-45degree.jpg", 0.5, 45.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m.jpg", 1.0, 0.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-1.0m-45degree.jpg", 1.0, 45.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m.jpg", 2.0, 0.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-2.0m-45degree.jpg", 2.0, 45.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m.jpg", 3.0, 0.0 }, - { "https://heyeuuu19.com/auto_aim/pnp/blue-3.0m-45degree.jpg", 3.0, 45.0 }, + { "blue-0.5m.jpg", 0.5, 0.0 }, + { "blue-0.5m-45degree.jpg", 0.5, 45.0 }, + { "blue-1.0m.jpg", 1.0, 0.0 }, + { "blue-1.0m-45degree.jpg", 1.0, 45.0 }, + { "blue-2.0m.jpg", 2.0, 0.0 }, + { "blue-2.0m-45degree.jpg", 2.0, 45.0 }, + { "blue-3.0m.jpg", 3.0, 0.0 }, + { "blue-3.0m-45degree.jpg", 3.0, 45.0 }, }; -// --- 网络/模型推理辅助函数 --- -static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp) { - size_t total_size = size * nmemb; - std::vector* buffer = static_cast*>(userp); - buffer->insert(buffer->end(), (uchar*)contents, (uchar*)contents + total_size); - return total_size; -} - -std::vector download_image_to_buffer(const std::string& url) { - CURL* curl; - CURLcode res; - std::vector buffer; - - curl_global_init(CURL_GLOBAL_DEFAULT); - curl = curl_easy_init(); - - if (curl) { - curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteCallback); - curl_easy_setopt(curl, CURLOPT_WRITEDATA, &buffer); - curl_easy_setopt(curl, CURLOPT_SSL_VERIFYPEER, 0L); - curl_easy_setopt(curl, CURLOPT_SSL_VERIFYHOST, 0L); - - res = curl_easy_perform(curl); - if (res != CURLE_OK) { - buffer.clear(); - } - - curl_easy_cleanup(curl); - } - curl_global_cleanup(); - - return buffer; -} +// --- 资源读取辅助函数 --- // --- 核心辅助函数：相机/装甲板参数 --- // 辅助函数：创建测试用的相机内参 PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, @@ -99,7 +82,7 @@ PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, // [Top Left, Top Right, Bottom Right, Bottom Left] std::array create_small_armor_shape() { return rmcs::kSmallArmorShapeOpenCV; } -std::array infer_armor_detection_from_url(std::string_view image_url) { +std::array infer_armor_detection_from_file(std::string_view filename) { using namespace rmcs::identifier; constexpr auto config_yaml = R"( @@ -128,14 +111,10 @@ std::array infer_armor_detection_from_url(std::string_view image_url throw std::runtime_error("Failed to configure OpenVinoNet: " + cfg_result.error()); } - const auto buffer = download_image_to_buffer(std::string { image_url }); - if (buffer.empty()) { - throw std::runtime_error("Failed to download image from url: " + std::string { image_url }); - } - auto cv_mat = cv::imdecode(buffer, cv::IMREAD_COLOR); + const auto full_path = asset_path(filename); + auto cv_mat = cv::imread(full_path.string(), cv::IMREAD_COLOR); if (cv_mat.empty()) { - throw std::runtime_error( - "Failed to decode image from buffer for url: " + std::string { image_url }); + throw std::runtime_error("Failed to read image: " + full_path.string()); } auto image = rmcs::Image {}; @@ -147,7 +126,7 @@ std::array infer_armor_detection_from_url(std::string_view image_url } const auto& armors = infer_result.value(); if (armors.empty()) { - throw std::runtime_error("No armor detected from image: " + std::string { image_url }); + throw std::runtime_error("No armor detected from image: " + full_path.string()); } const auto& armor = armors.front(); @@ -160,17 +139,6 @@ std::array infer_armor_detection_from_url(std::string_view image_url }; } -// 缓存推理结果，避免重复下载和推理 -std::array cached_detection(std::string_view url) { - static std::unordered_map> cache; - if (auto it = cache.find(std::string { url }); it != cache.end()) { - return it->second; - } - auto det = infer_armor_detection_from_url(url); - cache.emplace(url, det); - return det; -} - // --- PnP 结果处理辅助函数 --- /** @@ -222,20 +190,21 @@ class PnpSolverParameterizedTest : public ::testing::TestWithParam solution.input = create_test_input(); solution.input.armor_shape = create_small_armor_shape(); - const auto detection = cached_detection(test_case.url); + const auto detection = infer_armor_detection_from_file(test_case.filename); solution.input.armor_detection = detection; std::cerr << std::fixed << std::setprecision(1); - std::cerr << "[2D_POINTS] | URL: " << test_case.url << " | TL(" << detection[0].x << "," - << detection[0].y << ")" + std::cerr << "[2D_POINTS] | FILE: " << test_case.filename << " | TL(" << detection[0].x + << "," << detection[0].y << ")" << " TR(" << detection[1].x << "," << detection[1].y << ")" << " BR(" << detection[2].x << "," << detection[2].y << ")" << " BL(" << detection[3].x << "," << detection[3].y << ")" << std::endl; std::cerr << std::defaultfloat; // 恢复默认浮点格式 - SCOPED_TRACE(test_case.url); + SCOPED_TRACE(test_case.filename); - EXPECT_NO_THROW(solution.solve()) << "Pnp solve failed for URL: " << test_case.url; + EXPECT_NO_THROW(solution.solve()) + << "Pnp solve failed for file: " << asset_path(test_case.filename); // --- 解算结果处理 --- const auto& result = solution.result; @@ -253,7 +222,7 @@ class PnpSolverParameterizedTest : public ::testing::TestWithParam // 打印结构化报告 void PrintStructuredReport() const { std::cerr << std::fixed << std::setprecision(4); - std::cerr << "[TEST_REPORT] |" << std::left << std::setw(50) << test_case.url << "|"; + std::cerr << "[TEST_REPORT] |" << std::left << std::setw(50) << test_case.filename << "|"; // 距离信息 std::cerr << " DISTANCE: " << std::setw(6) << actual_distance << "m (Exp: " << std::setw(4) @@ -287,22 +256,18 @@ TEST_P(PnpSolverParameterizedTest, DistanceAndAngleAccuracy) { EXPECT_LT(this->distance_error, max_dist_error) << "Distance error (" << this->distance_error << "m) exceeded " << max_dist_error - << "m for " << GetParam().url; + << "m for " << GetParam().filename; // 2. 角度断言 (15度误差) EXPECT_LT(this->yaw_error, this->max_allowed_yaw_error_deg) << "Yaw error (" << this->yaw_error << "deg) exceeded " << this->max_allowed_yaw_error_deg - << "deg for " << GetParam().url; + << "deg for " << GetParam().filename; } // 注册参数化测试 INSTANTIATE_TEST_SUITE_P(AllImageTests, PnpSolverParameterizedTest, ::testing::ValuesIn(kPnpTestCases), [](const ::testing::TestParamInfo& info) { - std::string name = info.param.url; - size_t pos = name.find_last_of('/'); - if (pos != std::string::npos) { - name = name.substr(pos + 1); - } + std::string name = info.param.filename; std::replace(name.begin(), name.end(), '.', '_'); std::replace(name.begin(), name.end(), '-', '_'); return name; From 51f056e6f6bc536cc7f0e4273d561c3b8572a5ac Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 04:20:53 +0800 Subject: [PATCH 32/36] chore(ci): supply missing dependencies in CI/CD pipeline --- .github/workflows/gtest.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/gtest.yml b/.github/workflows/gtest.yml index f46551f8..85289b6f 100644 --- a/.github/workflows/gtest.yml +++ b/.github/workflows/gtest.yml @@ -25,6 +25,11 @@ jobs: repository: Alliance-Algorithm/rmcs_auto_aim_v2 path: ws/src/rmcs_auto_aim_v2 + - name: Install yq dependency + run: | + apt-get update + apt-get install -y yq + - name: Download test assets shell: bash run: | From b5b130320cecf3ee8098d2a289ef29babf2970e8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 05:04:40 +0800 Subject: [PATCH 33/36] fix(visual_armor,test,utility): minor fixes and general cleanup --- .../debug/visualization/armor_visualizer.cpp | 13 ++++++------ src/runtime.cpp | 1 - src/utility/math/conversion.hpp | 21 ++++++++++--------- test/CMakeLists.txt | 4 ++-- test/solve_pnp.cpp | 8 +++---- tool/CMakeLists.txt | 1 - 6 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index d98492c7..afc95912 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -1,6 +1,7 @@ #include "armor_visualizer.hpp" #include "utility/rclcpp/visual/armor.hpp" +#include "utility/robot/armor.hpp" using namespace rmcs::debug; using VisualArmor = rmcs::util::visual::Armor; @@ -41,7 +42,7 @@ struct ArmorVisualizer::Impl final { auto const config = VisualArmor::Config { .rclcpp = node.value().get(), .device = input.genre, - .camp = camp(input.color), + .camp = armor_color2camp_color(input.color), .id = input.id, .name = "solved_pnp_armor", .tf = "camera_link", @@ -61,11 +62,11 @@ struct ArmorVisualizer::Impl final { return true; } - static auto camp(ArmorColor const& color) -> CampColor { - if (color == ArmorColor::BLUE) return CampColor::BLUE; - if (color == ArmorColor::RED) return CampColor::RED; - return CampColor::UNKNOWN; - }; + // static auto camp(ArmorColor const& color) -> CampColor { + // if (color == ArmorColor::BLUE) return CampColor::BLUE; + // if (color == ArmorColor::RED) return CampColor::RED; + // return CampColor::UNKNOWN; + // }; static bool needs_rebuild(ArmorShadow shadow, Armor3D const& input) { return shadow.genre != input.genre || shadow.color != input.color || shadow.id != input.id; diff --git a/src/runtime.cpp b/src/runtime.cpp index 486b71ba..182a0f92 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -35,7 +35,6 @@ auto main() -> int { auto framerate = FramerateCounter {}; framerate.set_interval(5s); - framerate.set_interval(5s); /// Runtime /// diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp index e48fefed..a00820b6 100644 --- a/src/utility/math/conversion.hpp +++ b/src/utility/math/conversion.hpp @@ -5,17 +5,21 @@ namespace rmcs::util { +// OpenCV 与 ROS 坐标系之间的变换矩阵 +static const Eigen::Matrix3d kCoordTransformMatrix = + // clang-format off + (Eigen::Matrix3d() << 0, 0, 1, + -1, 0, 0, + 0,-1, 0).finished(); +// clang-format on + static inline auto opencv2ros_position(const Eigen::Vector3d& position) -> Eigen::Vector3d { auto result = Eigen::Vector3d(position.z(), -position.x(), -position.y()); return result; } static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotation_matrix) { - Eigen::Matrix3d t; - t << 0, 0, 1, // - -1, 0, 0, // - 0, -1, 0; // - return t * rotation_matrix * t.transpose(); + return kCoordTransformMatrix * rotation_matrix * kCoordTransformMatrix.transpose(); } static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& position) { @@ -24,11 +28,8 @@ static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& positio } static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotation_matrix) { - Eigen::Matrix3d t; - t << 0, 0, 1, // - -1, 0, 0, // - 0, -1, 0; // - return t.transpose() * rotation_matrix * t; + + return kCoordTransformMatrix.transpose() * rotation_matrix * kCoordTransformMatrix; } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 008f4ec6..dd33da70 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -123,7 +123,7 @@ if(HIKCAMERA_AVAILABLE) ) target_link_libraries( example_hikcamera - ${OpenCV_LIBS} + ${OpenCV_LIBRARIES} ${hikcamera_LIBRARIES} ) @@ -140,7 +140,7 @@ if(HIKCAMERA_AVAILABLE) target_link_libraries( example_streaming rclcpp::rclcpp - ${OpenCV_LIBS} + ${OpenCV_LIBRARIES} ${hikcamera_LIBRARIES} ) else() diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index 22e6ccc7..e36f05ba 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -29,9 +29,9 @@ static std::filesystem::path asset_root() { if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { return std::filesystem::path { env }; } - - const char* default_path = "/tmp/auto_aim"; - + + const char* default_path = "/tmp/auto_aim"; + return std::filesystem::path { default_path }; } @@ -67,7 +67,7 @@ PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, double k2 = -0.087667493884102, double k3 = 0.792381808294582) { auto distort_coeff = std::array { k1, k2, 0, 0, k3 }; - PnpSolution::Input input; + PnpSolution::Input input {}; input.camera_matrix = { { { fx, 0.0, cx }, { 0.0, fy, cy }, diff --git a/tool/CMakeLists.txt b/tool/CMakeLists.txt index 37e5875a..e54354a7 100644 --- a/tool/CMakeLists.txt +++ b/tool/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(yaml-cpp REQUIRED) find_package(OpenCV 4 REQUIRED) include_directories( From 4e05fba2d0c9d53fc31315ac81f7079593d4931d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 05:53:49 +0800 Subject: [PATCH 34/36] fix(pnp): correct 4-point order and decouple PnP failure handling & visualization --- src/kernel/pose_estimator.cpp | 7 ++++++- src/runtime.cpp | 18 +++++++----------- src/utility/math/solve_pnp/pnp_solution.cpp | 6 ++++-- src/utility/math/solve_pnp/pnp_solution.hpp | 2 +- src/utility/robot/armor.hpp | 2 +- 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 3a9a9318..90d93efd 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -80,7 +80,12 @@ struct PoseEstimator::Impl { pnp_solution.input.color = armor_color2camp_color(armor.color); std::ranges::copy(armor.corners(), pnp_solution.input.armor_detection.begin()); - pnp_solution.solve(); + auto solved = pnp_solution.solve(); + if (!solved) { + log.warn("solvePnP failed for armor {} ({} {})", i, + get_enum_name(armor.genre), get_enum_name(armor.color)); + return; + } auto armor_3d = Armor3D {}; armor_3d.genre = pnp_solution.result.genre; diff --git a/src/runtime.cpp b/src/runtime.cpp index 182a0f92..4ece5e70 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -96,6 +96,7 @@ auto main() -> int { for (const auto& armor_2d : *armors_2d) util::draw(*image, armor_2d); } + if (visualization.initialized()) { visualization.send_image(*image); } @@ -107,22 +108,17 @@ auto main() -> int { }); auto armors_3d = pose_estimator.solve_pnp(armors_2d); - if (!armors_3d.has_value()) { - continue; - } - // TODO: pose estimator - // TODO: predictor - // TODO: control + if (!armors_3d.has_value()) continue; if (visualization.initialized()) { - visualization.send_image(*image); - std::ignore = visualization.visualize_armors(*armors_3d); + visualization.visualize_armors(*armors_3d); } + // TODO: pose estimator + // TODO: predictor + // TODO: control + rclcpp_node.spin_once(); } - - rclcpp_node.spin_once(); } - rclcpp_node.shutdown(); } diff --git a/src/utility/math/solve_pnp/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp index 2a5ef776..9b4c76ab 100644 --- a/src/utility/math/solve_pnp/pnp_solution.cpp +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -10,7 +10,7 @@ #include using namespace rmcs::util; -auto PnpSolution::solve() noexcept -> void { +auto PnpSolution::solve() noexcept -> bool { const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); @@ -25,7 +25,7 @@ auto PnpSolution::solve() noexcept -> void { auto success = cv::solvePnP(armor_shape, armor_detection, camera_matrix, distort_coeff, rota_vec, tran_vec, false, cv::SOLVEPNP_IPPE); - if (!success) return; + if (!success) return false; auto tran_vec_eigen_opencv = Eigen::Vector3d {}; cv::cv2eigen(tran_vec, tran_vec_eigen_opencv); @@ -40,4 +40,6 @@ auto PnpSolution::solve() noexcept -> void { result.translation = opencv2ros_position(tran_vec_eigen_opencv); result.orientation = Eigen::Quaterniond(opencv2ros_rotation(rotation_eigen_opencv)).normalized(); + + return true; } diff --git a/src/utility/math/solve_pnp/pnp_solution.hpp b/src/utility/math/solve_pnp/pnp_solution.hpp index 638d1037..1499dcb0 100644 --- a/src/utility/math/solve_pnp/pnp_solution.hpp +++ b/src/utility/math/solve_pnp/pnp_solution.hpp @@ -28,6 +28,6 @@ struct PnpSolution { PnpSolution() noexcept = default; - auto solve() noexcept -> void; + auto solve() noexcept -> bool; }; } diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 3f91d8ef..a2bd7114 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -94,8 +94,8 @@ constexpr std::array kLargeArmorShapeRos { }; constexpr std::array kSmallArmorShapeRos { Point3d { 0.0, 0.0675, 0.028 }, // Top-left - Point3d { 0.0, -0.0675, 0.028 }, // Top-right Point3d { 0.0, -0.0675, -0.028 }, // Bottom-right + Point3d { 0.0, -0.0675, 0.028 }, // Top-right Point3d { 0.0, 0.0675, -0.028 } // Bottom-left }; } From 7d3d7f77caffe1bca5aad332731149b314eb7809 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 06:42:19 +0800 Subject: [PATCH 35/36] refactor(visualization): remove redundant visualization variables --- config/config.yaml | 1 - src/kernel/visualization.cpp | 4 ---- src/utility/image/armor.cpp | 1 + test/solve_pnp.cpp | 4 ++++ 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index c1f9f21c..ea3d446f 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -69,4 +69,3 @@ visualization: monitor_host: "127.0.0.1" monitor_port: "5000" stream_type: "RTP_JEPG" - show_armors: true diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 4591ccf8..f10b8d08 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -29,8 +29,6 @@ struct Visualization::Impl { util::string_t stream_type = "RTP_JEPG"; - bool show_armors = false; - static constexpr auto metas = std::tuple { &Config::framerate, "framerate", @@ -40,8 +38,6 @@ struct Visualization::Impl { "monitor_port", &Config::stream_type, "stream_type", - &Config::show_armors, - "show_armors", }; }; diff --git a/src/utility/image/armor.cpp b/src/utility/image/armor.cpp index 3472f1de..876e5978 100644 --- a/src/utility/image/armor.cpp +++ b/src/utility/image/armor.cpp @@ -47,6 +47,7 @@ auto draw(Image& canvas, const Armor2D& armor) noexcept -> void { cv::putText(opencv_mat, "TR", armor.tr, font, scale, white, thickness, cv::LINE_AA); cv::putText(opencv_mat, "BL", armor.bl, font, scale, white, thickness, cv::LINE_AA); cv::putText(opencv_mat, "BR", armor.br, font, scale, white, thickness, cv::LINE_AA); + cv::putText(opencv_mat, "TL", armor.tl, font, scale, white, thickness, cv::LINE_AA); auto info = std::format("{:.2f} {} {}", armor.confidence, genre, shape); cv::putText(opencv_mat, info, armor.tl + cv::Point2f { 0, -5 }, font, scale, white, thickness, diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index e36f05ba..28a99a37 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -25,6 +25,10 @@ using Eigen::Vector2d; using Eigen::Vector3d; // --- 资源路径 --- +// 测试资源路径配置: +// - 优先使用环境变量 TEST_ASSETS_ROOT +// - 未设置时默认使用 /tmp/auto_aim +// - 运行前需执行: cd test && ./download_assets.sh static std::filesystem::path asset_root() { if (const char* env = std::getenv("TEST_ASSETS_ROOT"); env && *env) { return std::filesystem::path { env }; From 89f54aa65ea8a2594b4cc946d519eccda2cc92cc Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 16 Dec 2025 06:57:02 +0800 Subject: [PATCH 36/36] fix(test): correct visualization test by assigning unique armor IDs --- tool/visualization.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tool/visualization.cpp b/tool/visualization.cpp index 66cf0402..2b0a7bc0 100644 --- a/tool/visualization.cpp +++ b/tool/visualization.cpp @@ -39,8 +39,11 @@ auto main() -> int { .name = "visual_test_armor", .tf = "camera_link", }; - std::ranges::for_each( - armors, [&](auto& armor) { armor = std::make_unique(config); }); + + for (auto i = 0; i < (int)armors.size(); ++i) { + config.id = i; + armors[i] = std::make_unique(config); + } } auto posture = std::make_unique( //