|
| 1 | +#include <gtest/gtest.h> |
| 2 | + |
| 3 | +#include <Eigen/Dense> |
| 4 | +#include <opencv2/opencv.hpp> |
| 5 | +#include <vector> |
| 6 | + |
| 7 | +#include "apriltag_pose_estimator/multi_tag_pose_estimator.hpp" |
| 8 | +#include "apriltag_pose_estimator/tag_config.hpp" |
| 9 | + |
| 10 | +namespace apriltag_pose_estimator { |
| 11 | + |
| 12 | +// ======================================================================== |
| 13 | +// Test Fixture |
| 14 | +// |
| 15 | +// 실제 카메라/ROS 없이 estimator만 단위 테스트하기 위한 공통 준비물. |
| 16 | +// 가상 카메라와 2-마커 그룹(base=0, marker1은 base 기준 +x 0.2m)을 정의하고, |
| 17 | +// 알려진 pose에서 3D 코너를 이미지로 투영해 "완벽한 검출 결과"를 합성한다. |
| 18 | +// 각 TEST_F는 이 클래스를 상속받아 아래 멤버/헬퍼를 그대로 사용한다. |
| 19 | +// ======================================================================== |
| 20 | +class MultiTagPoseEstimatorTest : public ::testing::Test { |
| 21 | + protected: |
| 22 | + // 각 테스트 시작 직전에 매번 호출됨(gtest 규약). |
| 23 | + // estimator를 새로 만들어 내부 상태(재사용 카운터, SLERP 필터)가 |
| 24 | + // 테스트 간에 섞이지 않도록 한다. |
| 25 | + void SetUp() override { |
| 26 | + // 가상 카메라 내부 파라미터 (640x480 해상도 가정, 렌즈 왜곡 없음) |
| 27 | + camera_matrix_ = cv::Mat::eye(3, 3, CV_64F); |
| 28 | + camera_matrix_.at<double>(0, 0) = 600.0; // fx |
| 29 | + camera_matrix_.at<double>(1, 1) = 600.0; // fy |
| 30 | + camera_matrix_.at<double>(0, 2) = 320.0; // cx |
| 31 | + camera_matrix_.at<double>(1, 2) = 240.0; // cy |
| 32 | + dist_coeffs_ = cv::Mat::zeros(5, 1, CV_64F); |
| 33 | + |
| 34 | + // 그룹 구성: 마커당 6개 오프셋(x, y, z, roll, pitch, yaw) |
| 35 | + // marker0(base): identity, marker1: base 기준 +x 0.2m |
| 36 | + marker_ids_ = {0, 1}; |
| 37 | + marker_offsets_ = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
| 38 | + 0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; |
| 39 | + |
| 40 | + estimator_ = std::make_unique<MultiTagPoseEstimator>( |
| 41 | + marker_ids_, marker_offsets_, tag_size_, camera_matrix_, dist_coeffs_); |
| 42 | + } |
| 43 | + |
| 44 | + // 주어진 base pose(rvec/tvec)에서 각 마커의 3D 코너를 이미지에 투영해 |
| 45 | + // 검출 결과를 합성한다. marker_x_offsets가 마커의 "실제" 위치이므로, |
| 46 | + // 설정값(0.2m)과 다르게 주면 재투영 오차가 큰 outlier 프레임이 된다. |
| 47 | + std::vector<TagDetection> makeDetections( |
| 48 | + const cv::Mat& rvec, const cv::Mat& tvec, |
| 49 | + const std::vector<float>& marker_x_offsets) { |
| 50 | + const float a = tag_size_ / 2.0f; |
| 51 | + // multi_tag_pose_estimator.cpp의 local_corners와 동일한 순서 |
| 52 | + const std::vector<cv::Point3f> local_corners = { |
| 53 | + {-a, +a, 0.0f}, {+a, +a, 0.0f}, {+a, -a, 0.0f}, {-a, -a, 0.0f}}; |
| 54 | + |
| 55 | + std::vector<TagDetection> detections; |
| 56 | + for (size_t i = 0; i < marker_ids_.size(); i++) { |
| 57 | + std::vector<cv::Point3f> pts3d; |
| 58 | + for (const auto& c : local_corners) { |
| 59 | + pts3d.emplace_back(c.x + marker_x_offsets[i], c.y, c.z); |
| 60 | + } |
| 61 | + std::vector<cv::Point2f> pts2d; |
| 62 | + cv::projectPoints(pts3d, rvec, tvec, camera_matrix_, dist_coeffs_, pts2d); |
| 63 | + |
| 64 | + TagDetection det; |
| 65 | + det.id = marker_ids_[i]; |
| 66 | + det.pose = Eigen::Matrix4f::Identity(); |
| 67 | + det.corners = pts2d; |
| 68 | + detections.push_back(det); |
| 69 | + } |
| 70 | + return detections; |
| 71 | + } |
| 72 | + |
| 73 | + // 설정된 offset(0.2m)과 일치하는 정상 프레임 |
| 74 | + std::vector<TagDetection> validDetections(const cv::Mat& rvec, const cv::Mat& tvec) { |
| 75 | + return makeDetections(rvec, tvec, {0.0f, 0.2f}); |
| 76 | + } |
| 77 | + |
| 78 | + // marker1이 설정보다 +0.15m 어긋난 outlier 프레임 (재투영 오차 > 15px 유발) |
| 79 | + std::vector<TagDetection> outlierDetections(const cv::Mat& rvec, const cv::Mat& tvec) { |
| 80 | + return makeDetections(rvec, tvec, {0.0f, 0.35f}); |
| 81 | + } |
| 82 | + |
| 83 | + // estimate() 호출 래퍼: rvec/tvec 출력 인자는 테스트에서 쓰지 않으므로 감춘다 |
| 84 | + bool runEstimate(const std::vector<TagDetection>& dets, cv::Mat& img, |
| 85 | + Eigen::Matrix4f& base_xf) { |
| 86 | + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); |
| 87 | + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F); |
| 88 | + return estimator_->estimate(dets, img, rvec, tvec, base_xf); |
| 89 | + } |
| 90 | + |
| 91 | + // 새 검정 프레임 (노드의 vis_image 역할) |
| 92 | + cv::Mat blankImage() { return cv::Mat::zeros(480, 640, CV_8UC3); } |
| 93 | + |
| 94 | + // 이미지에 아무것도 그려지지 않았는지 확인 (모든 픽셀이 0인지 검사) |
| 95 | + static bool imageUntouched(const cv::Mat& img) { |
| 96 | + cv::Scalar s = cv::sum(img); |
| 97 | + return (s[0] + s[1] + s[2] + s[3]) == 0.0; |
| 98 | + } |
| 99 | + |
| 100 | + std::vector<int> marker_ids_; |
| 101 | + std::vector<float> marker_offsets_; |
| 102 | + float tag_size_ = 0.1f; |
| 103 | + cv::Mat camera_matrix_; |
| 104 | + cv::Mat dist_coeffs_; |
| 105 | + std::unique_ptr<MultiTagPoseEstimator> estimator_; |
| 106 | +}; |
| 107 | + |
| 108 | +// ======================================================================== |
| 109 | +// (A) Outlier pose 재사용은 연속 max_pose_reuse_frames(5)회로 제한되어야 한다 |
| 110 | +// ======================================================================== |
| 111 | +TEST_F(MultiTagPoseEstimatorTest, OutlierPoseReuseIsBounded) { |
| 112 | + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); |
| 113 | + cv::Mat tvec = (cv::Mat_<double>(3, 1) << 0.0, 0.0, 0.5); |
| 114 | + |
| 115 | + Eigen::Matrix4f base_xf; |
| 116 | + cv::Mat img = blankImage(); |
| 117 | + |
| 118 | + // 정상 프레임 → 유효 pose 저장 |
| 119 | + ASSERT_TRUE(runEstimate(validDetections(rvec, tvec), img, base_xf)); |
| 120 | + |
| 121 | + // 연속 outlier 5프레임까지는 이전 pose 재사용(성공 유지) |
| 122 | + for (int k = 0; k < 5; k++) { |
| 123 | + img = blankImage(); |
| 124 | + EXPECT_TRUE(runEstimate(outlierDetections(rvec, tvec), img, base_xf)) |
| 125 | + << "reuse frame " << k + 1 << " should still succeed"; |
| 126 | + } |
| 127 | + |
| 128 | + // 6번째 연속 outlier부터는 실패로 보고해야 함 (무기한 재사용 금지) |
| 129 | + img = blankImage(); |
| 130 | + EXPECT_FALSE(runEstimate(outlierDetections(rvec, tvec), img, base_xf)) |
| 131 | + << "stale pose must not be reused beyond the reuse limit"; |
| 132 | + |
| 133 | + // 이후 정상 프레임이 오면 다시 추정 가능해야 함 |
| 134 | + img = blankImage(); |
| 135 | + EXPECT_TRUE(runEstimate(validDetections(rvec, tvec), img, base_xf)); |
| 136 | +} |
| 137 | + |
| 138 | +// ======================================================================== |
| 139 | +// (B) 재사용된(실측이 아닌) pose로는 축을 그리지 않아야 한다 |
| 140 | +// ======================================================================== |
| 141 | +TEST_F(MultiTagPoseEstimatorTest, NoAxesDrawnForReusedPose) { |
| 142 | + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); |
| 143 | + cv::Mat tvec = (cv::Mat_<double>(3, 1) << 0.0, 0.0, 0.5); |
| 144 | + |
| 145 | + Eigen::Matrix4f base_xf; |
| 146 | + |
| 147 | + // 정상 프레임: 축이 그려져야 함 (sanity check) |
| 148 | + cv::Mat img_valid = blankImage(); |
| 149 | + ASSERT_TRUE(runEstimate(validDetections(rvec, tvec), img_valid, base_xf)); |
| 150 | + EXPECT_FALSE(imageUntouched(img_valid)) << "axes should be drawn for a measured pose"; |
| 151 | + |
| 152 | + // outlier 프레임(재사용 pose): 이미지에 아무것도 그리지 않아야 함 |
| 153 | + cv::Mat img_reuse = blankImage(); |
| 154 | + ASSERT_TRUE(runEstimate(outlierDetections(rvec, tvec), img_reuse, base_xf)); |
| 155 | + EXPECT_TRUE(imageUntouched(img_reuse)) |
| 156 | + << "stale (reused) pose must not be visualized on the latest image"; |
| 157 | +} |
| 158 | + |
| 159 | +// ======================================================================== |
| 160 | +// (C) 검출 공백(연속 미검출) 후에는 필터가 리셋되어 옛 pose와 블렌딩되지 않아야 한다 |
| 161 | +// ======================================================================== |
| 162 | +TEST_F(MultiTagPoseEstimatorTest, FilterResetsAfterDetectionGap) { |
| 163 | + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F); |
| 164 | + cv::Mat tvec1 = (cv::Mat_<double>(3, 1) << 0.0, 0.0, 0.5); |
| 165 | + cv::Mat tvec2 = (cv::Mat_<double>(3, 1) << 0.3, 0.0, 0.5); // +x 0.3m 이동 |
| 166 | + |
| 167 | + Eigen::Matrix4f base_xf; |
| 168 | + cv::Mat img = blankImage(); |
| 169 | + |
| 170 | + // P1에서 정상 추정 → 필터 상태가 P1에 초기화됨 |
| 171 | + ASSERT_TRUE(runEstimate(validDetections(rvec, tvec1), img, base_xf)); |
| 172 | + |
| 173 | + // 5프레임 연속 미검출 (marker1 없음 → estimate 실패) |
| 174 | + for (int k = 0; k < 5; k++) { |
| 175 | + auto only_base = validDetections(rvec, tvec1); |
| 176 | + only_base.resize(1); // marker0만 남김 |
| 177 | + img = blankImage(); |
| 178 | + ASSERT_FALSE(runEstimate(only_base, img, base_xf)); |
| 179 | + } |
| 180 | + |
| 181 | + // 재등장한 P2 프레임: 옛 P1 상태와 블렌딩되지 않고 P2를 그대로 출력해야 함 |
| 182 | + img = blankImage(); |
| 183 | + ASSERT_TRUE(runEstimate(validDetections(rvec, tvec2), img, base_xf)); |
| 184 | + EXPECT_NEAR(base_xf(0, 3), 0.3f, 1e-3f) |
| 185 | + << "pose after detection gap must not be blended with the stale filter state"; |
| 186 | +} |
| 187 | + |
| 188 | +} // namespace apriltag_pose_estimator |
0 commit comments