refactor: optimize IbvsController&TagRelativeTarget3D

cmvr-es/CMakeLists.txt

@@ -5,9 +5,9 @@ add_subdirectory(device_manager)
 add_subdirectory(monitor)
 add_subdirectory(monitor_manager)
 add_subdirectory(service)
+add_subdirectory(perception)
 add_subdirectory(controller)
 add_subdirectory(planner)
-add_subdirectory(perception)
 
 add_subdirectory(ik_solver)
 add_subdirectory(data_center)

cmvr-es/common/CMakeLists.txt

@@ -18,6 +18,9 @@ target_include_directories(common PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(common PUBLIC
         cmvr_es::proto
+        opencv_core
+        opencv_imgproc
+        opencv_highgui
         avformat
         avdevice
         avutil
@@ -26,4 +29,20 @@ target_link_libraries(common PUBLIC
 )
 
 add_library(cmvr_es::common ALIAS common)
-install(TARGETS common LIBRARY DESTINATION lib)
+install(TARGETS common LIBRARY DESTINATION lib)
+
+add_executable(image_display_test
+        utils/visualization/image_display_test.cpp
+)
+
+target_link_libraries(image_display_test
+        PRIVATE
+        cmvr_es::common
+        cmvr_es::perception
+        cmvr_es::device::realsense_camera
+        cmvr_es::proto
+        glog
+        gtest
+        gtest_main
+        pthread
+)

cmvr-es/common/utils/visualization/image_display.h

@@ -0,0 +1,609 @@
+#pragma once
+
+#include <algorithm>
+#include <cstdint>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace cmvr::common {
+
+/**
+ * @brief 图像像素坐标。
+ *
+ * 坐标系约定：
+ * - `u`：图像列坐标，向右为正，单位像素。
+ * - `v`：图像行坐标，向下为正，单位像素。
+ */
+struct Pixel {
+    int u{0}; // 图像像素列坐标，单位像素。
+    int v{0}; // 图像像素行坐标，单位像素。
+};
+
+/**
+ * @brief BGR 颜色定义。
+ */
+struct Color {
+    uint8_t b{0}; // 蓝色通道，范围 [0, 255]。
+    uint8_t g{255}; // 绿色通道，范围 [0, 255]。
+    uint8_t r{0}; // 红色通道，范围 [0, 255]。
+
+    /**
+     * @brief 转为 OpenCV 的 `cv::Scalar`。
+     */
+    cv::Scalar toCvScalar() const {
+        return cv::Scalar(static_cast<double>(b),
+                          static_cast<double>(g),
+                          static_cast<double>(r));
+    }
+
+    /**
+     * @brief 预定义红色。
+     */
+    static Color red() { return Color{0, 0, 255}; }
+
+    /**
+     * @brief 预定义绿色。
+     */
+    static Color green() { return Color{0, 255, 0}; }
+
+    /**
+     * @brief 预定义蓝色。
+     */
+    static Color blue() { return Color{255, 0, 0}; }
+
+    /**
+     * @brief 预定义黄色。
+     */
+    static Color yellow() { return Color{0, 255, 255}; }
+
+    /**
+     * @brief 预定义白色。
+     */
+    static Color white() { return Color{255, 255, 255}; }
+
+    /**
+     * @brief 预定义黑色。
+     */
+    static Color black() { return Color{0, 0, 0}; }
+};
+
+/**
+ * @brief 图像上的文本叠加配置。
+ */
+struct TextOverlay {
+    std::string text; // 需要显示的字符串内容。
+    Pixel position_px{}; // 文本左下角在图像像素坐标系中的位置。
+    Color color{Color::green()}; // 文本颜色。
+    double font_scale{0.7}; // OpenCV 字体缩放系数。
+    int thickness{2}; // 文本线宽，单位像素。
+    int font_face{cv::FONT_HERSHEY_SIMPLEX}; // OpenCV 字体类型。
+    bool draw_background{false}; // 是否绘制文本背景框。
+    Color background_color{Color::black()}; // 文本背景框颜色。
+    int background_padding_px{2}; // 文本背景框四周留白，单位像素。
+};
+
+/**
+ * @brief 图像上的点叠加配置。
+ */
+struct PointOverlay {
+    Pixel position_px{}; // 点中心在图像像素坐标系中的位置。
+    Color color{Color::red()}; // 点颜色。
+    int radius_px{5}; // 圆点半径，单位像素。
+    int thickness{2}; // 线宽，`-1` 表示填充。
+    std::string label; // 点旁边附带显示的字符串；为空时不显示。
+    Pixel label_offset_px{8, -8}; // 标签相对点中心的像素偏移。
+    double label_font_scale{0.6}; // 点标签字体缩放系数。
+    int label_thickness{2}; // 点标签线宽，单位像素。
+};
+
+/**
+ * @brief 图像上的圆圈叠加配置。
+ */
+struct CircleOverlay {
+    Pixel center_px{}; // 圆心在图像像素坐标系中的位置。
+    int radius_px{10}; // 圆半径，单位像素。
+    Color color{Color::yellow()}; // 圆圈颜色。
+    int thickness{2}; // 圆圈线宽，单位像素，`-1` 表示填充。
+};
+
+/**
+ * @brief 图像上的十字叠加配置。
+ */
+struct CrossOverlay {
+    Pixel center_px{}; // 十字中心在图像像素坐标系中的位置。
+    int arm_length_px{8}; // 单侧十字臂长度，单位像素。
+    Color color{Color::blue()}; // 十字颜色。
+    int thickness{2}; // 十字线宽，单位像素。
+};
+
+/**
+ * @brief 通用图像叠加显示接口。
+ *
+ * 该接口不依赖任何感知或跟踪类，只抽象三类输入：
+ * - 待显示的图像数据；
+ * - 文本叠加项；
+ * - 点叠加项。
+ */
+class ImageDisplay {
+public:
+    virtual ~ImageDisplay() = default;
+
+    /**
+     * @brief 设置窗口名称。
+     * @param window_name 显示窗口名称。
+     */
+    virtual void setWindowName(const std::string& window_name) = 0;
+
+    /**
+     * @brief 设置待显示的图像。
+     * @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
+     */
+    virtual void setImage(const cv::Mat& image) = 0;
+
+    /**
+     * @brief 清空所有叠加项，但保留当前图像。
+     */
+    virtual void clearOverlays() = 0;
+
+    /**
+     * @brief 添加文本叠加项。
+     * @param overlay 文本叠加配置。
+     */
+    virtual void showText(const TextOverlay& overlay) = 0;
+
+    /**
+     * @brief 按位置、字符串、颜色和大小直接添加文本。
+     * @param text 需要显示的字符串内容。
+     * @param position_px 文本左下角在图像像素坐标系中的位置。
+     * @param color 文本颜色。
+     * @param font_scale OpenCV 字体缩放系数。
+     * @param thickness 文本线宽，单位像素。
+     */
+    virtual void showText(const std::string& text,
+                          const Pixel& position_px,
+                          const Color& color,
+                          double font_scale = 0.7,
+                          int thickness = 2) = 0;
+
+    /**
+     * @brief 添加点叠加项。
+     * @param overlay 点叠加配置。
+     */
+    virtual void showPoint(const PointOverlay& overlay) = 0;
+
+    /**
+     * @brief 按位置、颜色和大小直接添加点。
+     * @param position_px 点中心在图像像素坐标系中的位置。
+     * @param color 点颜色。
+     * @param radius_px 圆点半径，单位像素。
+     * @param thickness 线宽，`-1` 表示填充。
+     * @param label 点旁边附带显示的字符串；为空时不显示。
+     */
+    virtual void showPoint(const Pixel& position_px,
+                           const Color& color,
+                           int radius_px = 5,
+                           int thickness = 2,
+                           const std::string& label = {}) = 0;
+
+    /**
+     * @brief 显示圆圈叠加项。
+     * @param overlay 圆圈叠加配置。
+     */
+    virtual void showCircle(const CircleOverlay& overlay) = 0;
+
+    /**
+     * @brief 按位置、半径、颜色和线宽直接显示圆圈。
+     * @param center_px 圆心在图像像素坐标系中的位置。
+     * @param radius_px 圆半径，单位像素。
+     * @param color 圆圈颜色。
+     * @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
+     */
+    virtual void showCircle(const Pixel& center_px,
+                            int radius_px,
+                            const Color& color,
+                            int thickness = 2) = 0;
+
+    /**
+     * @brief 显示十字叠加项。
+     * @param overlay 十字叠加配置。
+     */
+    virtual void showCross(const CrossOverlay& overlay) = 0;
+
+    /**
+     * @brief 按位置、尺寸、颜色和线宽直接显示十字。
+     * @param center_px 十字中心在图像像素坐标系中的位置。
+     * @param arm_length_px 单侧十字臂长度，单位像素。
+     * @param color 十字颜色。
+     * @param thickness 十字线宽，单位像素。
+     */
+    virtual void showCross(const Pixel& center_px,
+                           int arm_length_px,
+                           const Color& color,
+                           int thickness = 2) = 0;
+
+    /**
+     * @brief 判断当前是否已有待显示图像。
+     */
+    virtual bool hasImage() const = 0;
+
+    /**
+     * @brief 将当前图像和叠加项渲染到输出图像。
+     * @param rendered_image 输出的渲染结果图像，BGR 三通道。
+     * @return 渲染成功返回 `true`。
+     */
+    virtual bool render(cv::Mat& rendered_image) const = 0;
+
+    /**
+     * @brief 显示当前渲染结果。
+     * @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
+     * @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
+     */
+    virtual int show(int wait_key_ms = 1) = 0;
+
+    /**
+     * @brief 关闭显示窗口。
+     */
+    virtual void close() = 0;
+};
+
+/**
+ * @brief 基于 OpenCV HighGUI 的图像叠加显示实现。
+ */
+class OpenCvImageCvDisplay final : public ImageDisplay {
+public:
+    /**
+     * @brief 构造显示器。
+     * @param window_name 显示窗口名称。
+     */
+    explicit OpenCvImageCvDisplay(std::string window_name = "image_overlay")
+        : window_name_(std::move(window_name)) {}
+
+    /**
+     * @brief 设置窗口名称。
+     * @param window_name 显示窗口名称。
+     */
+    void setWindowName(const std::string& window_name) override {
+        window_name_ = window_name;
+    }
+
+    /**
+     * @brief 设置待显示图像。
+     * @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
+     */
+    void setImage(const cv::Mat& image) override {
+        image_ = image.clone();
+    }
+
+    /**
+     * @brief 清空所有叠加项，但保留当前图像。
+     */
+    void clearOverlays() override {
+        texts_.clear();
+        points_.clear();
+        circles_.clear();
+        crosses_.clear();
+    }
+
+    /**
+     * @brief 添加文本叠加项。
+     * @param overlay 文本叠加配置。
+     */
+    void showText(const TextOverlay& overlay) override {
+        texts_.push_back(overlay);
+    }
+
+    /**
+     * @brief 按位置、字符串、颜色和大小直接添加文本。
+     * @param text 需要显示的字符串内容。
+     * @param position_px 文本左下角在图像像素坐标系中的位置。
+     * @param color 文本颜色。
+     * @param font_scale OpenCV 字体缩放系数。
+     * @param thickness 文本线宽，单位像素。
+     */
+    void showText(const std::string& text,
+                  const Pixel& position_px,
+                  const Color& color,
+                  double font_scale = 0.7,
+                  int thickness = 2) override {
+        TextOverlay overlay;
+        overlay.text = text;
+        overlay.position_px = position_px;
+        overlay.color = color;
+        overlay.font_scale = font_scale;
+        overlay.thickness = thickness;
+        showText(overlay);
+    }
+
+    /**
+     * @brief 添加点叠加项。
+     * @param overlay 点叠加配置。
+     */
+    void showPoint(const PointOverlay& overlay) override {
+        points_.push_back(overlay);
+    }
+
+    /**
+     * @brief 按位置、颜色和大小直接添加点。
+     * @param position_px 点中心在图像像素坐标系中的位置。
+     * @param color 点颜色。
+     * @param radius_px 圆点半径，单位像素。
+     * @param thickness 线宽，`-1` 表示填充。
+     * @param label 点旁边附带显示的字符串；为空时不显示。
+     */
+    void showPoint(const Pixel& position_px,
+                   const Color& color,
+                   int radius_px = 5,
+                   int thickness = 2,
+                   const std::string& label = {}) override {
+        PointOverlay overlay;
+        overlay.position_px = position_px;
+        overlay.color = color;
+        overlay.radius_px = radius_px;
+        overlay.thickness = thickness;
+        overlay.label = label;
+        showPoint(overlay);
+    }
+
+    /**
+     * @brief 显示圆圈叠加项。
+     * @param overlay 圆圈叠加配置。
+     */
+    void showCircle(const CircleOverlay& overlay) override {
+        circles_.push_back(overlay);
+    }
+
+    /**
+     * @brief 按位置、半径、颜色和线宽直接显示圆圈。
+     * @param center_px 圆心在图像像素坐标系中的位置。
+     * @param radius_px 圆半径，单位像素。
+     * @param color 圆圈颜色。
+     * @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
+     */
+    void showCircle(const Pixel& center_px,
+                    int radius_px,
+                    const Color& color,
+                    int thickness = 2) override {
+        CircleOverlay overlay;
+        overlay.center_px = center_px;
+        overlay.radius_px = radius_px;
+        overlay.color = color;
+        overlay.thickness = thickness;
+        showCircle(overlay);
+    }
+
+    /**
+     * @brief 显示十字叠加项。
+     * @param overlay 十字叠加配置。
+     */
+    void showCross(const CrossOverlay& overlay) override {
+        crosses_.push_back(overlay);
+    }
+
+    /**
+     * @brief 按位置、尺寸、颜色和线宽直接显示十字。
+     * @param center_px 十字中心在图像像素坐标系中的位置。
+     * @param arm_length_px 单侧十字臂长度，单位像素。
+     * @param color 十字颜色。
+     * @param thickness 十字线宽，单位像素。
+     */
+    void showCross(const Pixel& center_px,
+                   int arm_length_px,
+                   const Color& color,
+                   int thickness = 2) override {
+        CrossOverlay overlay;
+        overlay.center_px = center_px;
+        overlay.arm_length_px = arm_length_px;
+        overlay.color = color;
+        overlay.thickness = thickness;
+        showCross(overlay);
+    }
+
+    /**
+     * @brief 判断当前是否已有待显示图像。
+     */
+    bool hasImage() const override {
+        return !image_.empty();
+    }
+
+    /**
+     * @brief 将当前图像和叠加项渲染到输出图像。
+     * @param rendered_image 输出的渲染结果图像，BGR 三通道。
+     * @return 渲染成功返回 `true`。
+     */
+    bool render(cv::Mat& rendered_image) const override {
+        if (!toBgrImage(image_, rendered_image)) {
+            return false;
+        }
+
+        for (const CircleOverlay& overlay : circles_) {
+            drawCircle(rendered_image, overlay);
+        }
+        for (const CrossOverlay& overlay : crosses_) {
+            drawCross(rendered_image, overlay);
+        }
+        for (const PointOverlay& overlay : points_) {
+            drawPoint(rendered_image, overlay);
+        }
+        for (const TextOverlay& overlay : texts_) {
+            drawText(rendered_image, overlay);
+        }
+        return true;
+    }
+
+    /**
+     * @brief 显示当前渲染结果。
+     * @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
+     * @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
+     */
+    int show(int wait_key_ms = 1) override {
+        cv::Mat rendered_image;
+        if (!render(rendered_image)) {
+            return -1;
+        }
+
+        if (!window_created_) {
+            cv::namedWindow(window_name_, cv::WINDOW_NORMAL);
+            window_created_ = true;
+        }
+        cv::imshow(window_name_, rendered_image);
+        return cv::waitKey(wait_key_ms);
+    }
+
+    /**
+     * @brief 关闭显示窗口。
+     */
+    void close() override {
+        if (!window_created_) {
+            return;
+        }
+        cv::destroyWindow(window_name_);
+        window_created_ = false;
+    }
+
+private:
+    /**
+     * @brief 将输入图像转换为可叠加绘制的 BGR 三通道图像。
+     * @param input 输入图像。
+     * @param output 输出 BGR 图像。
+     * @return 转换成功返回 `true`。
+     */
+    static bool toBgrImage(const cv::Mat& input, cv::Mat& output) {
+        if (input.empty()) {
+            return false;
+        }
+
+        if (input.channels() == 1) {
+            cv::cvtColor(input, output, cv::COLOR_GRAY2BGR);
+            return true;
+        }
+        if (input.channels() == 3) {
+            output = input.clone();
+            return true;
+        }
+        if (input.channels() == 4) {
+            cv::cvtColor(input, output, cv::COLOR_BGRA2BGR);
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 在图像上绘制文本叠加项。
+     * @param image 目标图像，BGR 三通道。
+     * @param overlay 文本叠加配置。
+     */
+    static void drawText(cv::Mat& image, const TextOverlay& overlay) {
+        const cv::Point origin(overlay.position_px.u, overlay.position_px.v);
+        const int thickness = std::max(1, overlay.thickness);
+        const double font_scale = std::max(0.0, overlay.font_scale);
+
+        if (overlay.draw_background) {
+            int baseline = 0;
+            const cv::Size text_size = cv::getTextSize(overlay.text,
+                                                       overlay.font_face,
+                                                       font_scale,
+                                                       thickness,
+                                                       &baseline);
+            const int pad = std::max(0, overlay.background_padding_px);
+            const cv::Point top_left(origin.x - pad, origin.y - text_size.height - pad);
+            const cv::Point bottom_right(origin.x + text_size.width + pad, origin.y + baseline + pad);
+            cv::rectangle(image,
+                          top_left,
+                          bottom_right,
+                          overlay.background_color.toCvScalar(),
+                          cv::FILLED);
+        }
+
+        cv::putText(image,
+                    overlay.text,
+                    origin,
+                    overlay.font_face,
+                    font_scale,
+                    overlay.color.toCvScalar(),
+                    thickness,
+                    cv::LINE_AA);
+    }
+
+    /**
+     * @brief 在图像上绘制点叠加项。
+     * @param image 目标图像，BGR 三通道。
+     * @param overlay 点叠加配置。
+     */
+    static void drawPoint(cv::Mat& image, const PointOverlay& overlay) {
+        const cv::Point center(overlay.position_px.u, overlay.position_px.v);
+        const int radius = std::max(1, overlay.radius_px);
+        cv::circle(image,
+                   center,
+                   radius,
+                   overlay.color.toCvScalar(),
+                   overlay.thickness,
+                   cv::LINE_AA);
+
+        if (!overlay.label.empty()) {
+            TextOverlay label_overlay;
+            label_overlay.text = overlay.label;
+            label_overlay.position_px = Pixel{center.x + overlay.label_offset_px.u,
+                                              center.y + overlay.label_offset_px.v};
+            label_overlay.color = overlay.color;
+            label_overlay.font_scale = overlay.label_font_scale;
+            label_overlay.thickness = overlay.label_thickness;
+            drawText(image, label_overlay);
+        }
+    }
+
+    /**
+     * @brief 在图像上绘制圆圈叠加项。
+     * @param image 目标图像，BGR 三通道。
+     * @param overlay 圆圈叠加配置。
+     */
+    static void drawCircle(cv::Mat& image, const CircleOverlay& overlay) {
+        const cv::Point center(overlay.center_px.u, overlay.center_px.v);
+        const int radius = std::max(1, overlay.radius_px);
+        const int thickness = (overlay.thickness == 0) ? 1 : overlay.thickness;
+        cv::circle(image,
+                   center,
+                   radius,
+                   overlay.color.toCvScalar(),
+                   thickness,
+                   cv::LINE_AA);
+    }
+
+    /**
+     * @brief 在图像上绘制十字叠加项。
+     * @param image 目标图像，BGR 三通道。
+     * @param overlay 十字叠加配置。
+     */
+    static void drawCross(cv::Mat& image, const CrossOverlay& overlay) {
+        const cv::Point center(overlay.center_px.u, overlay.center_px.v);
+        const int arm_length = std::max(1, overlay.arm_length_px);
+        const int thickness = std::max(1, overlay.thickness);
+        const cv::Scalar color = overlay.color.toCvScalar();
+        cv::line(image,
+                 cv::Point(center.x - arm_length, center.y),
+                 cv::Point(center.x + arm_length, center.y),
+                 color,
+                 thickness,
+                 cv::LINE_AA);
+        cv::line(image,
+                 cv::Point(center.x, center.y - arm_length),
+                 cv::Point(center.x, center.y + arm_length),
+                 color,
+                 thickness,
+                 cv::LINE_AA);
+    }
+
+    std::string window_name_; // 显示窗口名称。
+    cv::Mat image_; // 当前待显示图像。
+    std::vector<TextOverlay> texts_; // 当前待绘制的文本叠加列表。
+    std::vector<PointOverlay> points_; // 当前待绘制的点叠加列表。
+    std::vector<CircleOverlay> circles_; // 当前待绘制的圆圈叠加列表。
+    std::vector<CrossOverlay> crosses_; // 当前待绘制的十字叠加列表。
+    bool window_created_{false}; // 显示窗口是否已经创建。
+};
+
+} // namespace cmvr::common

cmvr-es/common/utils/visualization/image_display_test.cpp

@@ -0,0 +1,373 @@
+#include "common/utils/visualization/image_display.h"
+#include "common/utils/image/image_process.h"
+#include "devices/camera/realsense_camera/include/realsense_camera.h"
+#include "perception/include/tag_relative_target_3d.h"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <iostream>
+#include <opencv2/imgproc.hpp>
+#include <string>
+
+namespace {
+
+cv::Mat makeBlankImage(int width = 160, int height = 120) {
+    return cv::Mat(height, width, CV_8UC3, cv::Scalar(0, 0, 0));
+}
+
+constexpr const char* kRsSerial = "243122074587";
+constexpr double kTagSizeM = 0.01975;
+constexpr int kRsWidth = 1280;
+constexpr int kRsHeight = 720;
+constexpr int kRsFps = 30;
+constexpr int kTargetU = -1; // negative means image center
+constexpr int kTargetV = -1; // negative means image center
+constexpr int kDisplayFrames = 1200;
+constexpr const char* kTrackingWindowName = "ImageDisplayTagTrackingTest";
+
+int countNonZeroPixelsInRoi(const cv::Mat& image, const cv::Rect& roi) {
+    const cv::Rect image_rect(0, 0, image.cols, image.rows);
+    const cv::Rect clipped = roi & image_rect;
+    if (clipped.width <= 0 || clipped.height <= 0) {
+        return 0;
+    }
+
+    cv::Mat gray;
+    cv::cvtColor(image(clipped), gray, cv::COLOR_BGR2GRAY);
+    return cv::countNonZero(gray);
+}
+
+bool projectTargetToPixel(const Eigen::Vector3d& p_c,
+                          const cmvr::device::Rs2Intrinsics& intrinsics,
+                          cmvr::common::Pixel& pixel_out) {
+    Eigen::Vector2d uv = Eigen::Vector2d::Zero();
+    if (!cmvr::ImageProcess::projectCameraPointToPixel(intrinsics, p_c, uv)) {
+        return false;
+    }
+
+    pixel_out.u = static_cast<int>(std::lround(uv.x()));
+    pixel_out.v = static_cast<int>(std::lround(uv.y()));
+    return true;
+}
+
+} // namespace
+
+TEST(OpenCvImageCvDisplayTest, RenderFailsWithoutImage) {
+    cmvr::common::OpenCvImageCvDisplay display;
+
+    cv::Mat rendered;
+    EXPECT_FALSE(display.render(rendered));
+}
+
+TEST(OpenCvImageCvDisplayTest, RenderPointChangesCenterRegion) {
+    cmvr::common::OpenCvImageCvDisplay display;
+    display.setImage(makeBlankImage());
+    display.showPoint(cmvr::common::Pixel{60, 40},
+                      cmvr::common::Color::red(),
+                      5,
+                      -1,
+                      "target");
+
+    cv::Mat rendered;
+    ASSERT_TRUE(display.render(rendered));
+    ASSERT_FALSE(rendered.empty());
+
+    const cv::Vec3b center = rendered.at<cv::Vec3b>(40, 60);
+    EXPECT_GT(static_cast<int>(center[2]), 150);
+    EXPECT_LT(static_cast<int>(center[1]), 80);
+    EXPECT_LT(static_cast<int>(center[0]), 80);
+}
+
+TEST(OpenCvImageCvDisplayTest, RenderCircleChangesExpectedArcRegion) {
+    cmvr::common::OpenCvImageCvDisplay display;
+    display.setImage(makeBlankImage());
+    display.showCircle(cmvr::common::Pixel{60, 40},
+                       12,
+                       cmvr::common::Color::yellow(),
+                       2);
+
+    cv::Mat rendered;
+    ASSERT_TRUE(display.render(rendered));
+    ASSERT_FALSE(rendered.empty());
+
+    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(58, 26, 5, 5)), 0);
+    EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 39, 3, 3)), 0);
+}
+
+TEST(OpenCvImageCvDisplayTest, RenderCrossChangesHorizontalAndVerticalArms) {
+    cmvr::common::OpenCvImageCvDisplay display;
+    display.setImage(makeBlankImage());
+    display.showCross(cmvr::common::Pixel{60, 40},
+                      10,
+                      cmvr::common::Color::blue(),
+                      2);
+
+    cv::Mat rendered;
+    ASSERT_TRUE(display.render(rendered));
+    ASSERT_FALSE(rendered.empty());
+
+    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(49, 39, 5, 3)), 0);
+    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 29, 3, 5)), 0);
+}
+
+TEST(OpenCvImageCvDisplayTest, RenderTextChangesRequestedRegion) {
+    cmvr::common::OpenCvImageCvDisplay display;
+    display.setImage(makeBlankImage());
+
+    cmvr::common::TextOverlay overlay;
+    overlay.text = "tracked tag: 7";
+    overlay.position_px = cmvr::common::Pixel{10, 30};
+    overlay.color = cmvr::common::Color::white();
+    overlay.draw_background = true;
+
+    display.showText(overlay);
+
+    cv::Mat rendered;
+    ASSERT_TRUE(display.render(rendered));
+    ASSERT_FALSE(rendered.empty());
+
+    EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(5, 5, 120, 35)), 0);
+}
+
+TEST(OpenCvImageCvDisplayTest, ClearOverlaysRemovesAllRenderedGeometry) {
+    cmvr::common::OpenCvImageCvDisplay display;
+    display.setImage(makeBlankImage());
+    display.showPoint(cmvr::common::Pixel{60, 40},
+                      cmvr::common::Color::red(),
+                      5,
+                      -1,
+                      "target");
+    display.showCircle(cmvr::common::Pixel{60, 40},
+                       12,
+                       cmvr::common::Color::yellow(),
+                       2);
+    display.showCross(cmvr::common::Pixel{60, 40},
+                      10,
+                      cmvr::common::Color::blue(),
+                      2);
+    display.showText("tracked tag: 7",
+                     cmvr::common::Pixel{10, 30},
+                     cmvr::common::Color::white(),
+                     0.7,
+                     2);
+
+    display.clearOverlays();
+
+    cv::Mat rendered;
+    ASSERT_TRUE(display.render(rendered));
+    ASSERT_FALSE(rendered.empty());
+    EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(0, 0, rendered.cols, rendered.rows)), 0);
+}
+
+TEST(OpenCvImageCvDisplayTest, ShowDisplaysWindowIfHighGuiIsAvailable) {
+    constexpr const char* kWindowName = "OpenCvImageCvDisplayTest";
+    cmvr::common::OpenCvImageCvDisplay display(kWindowName);
+
+    bool window_enabled = true;
+    try {
+        cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
+        cv::imshow(kWindowName, cv::Mat(120, 160, CV_8UC3, cv::Scalar(20, 20, 20)));
+        cv::waitKey(1);
+    } catch (const cv::Exception& e) {
+        window_enabled = false;
+        std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
+    }
+
+    if (!window_enabled) {
+        GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
+    }
+
+    for (int frame = 0; frame < 1200; ++frame) {
+        cv::Mat image(240, 320, CV_8UC3, cv::Scalar(20, 20, 20));
+        display.setImage(image);
+        display.clearOverlays();
+
+        const int center_u = 80 + frame;
+        const int center_v = 120;
+
+        display.showPoint(cmvr::common::Pixel{center_u, center_v},
+                          cmvr::common::Color::red(),
+                          5,
+                          -1,
+                          "target");
+        display.showCircle(cmvr::common::Pixel{center_u, center_v},
+                           18,
+                           cmvr::common::Color::yellow(),
+                           2);
+        display.showCross(cmvr::common::Pixel{center_u, center_v},
+                          10,
+                          cmvr::common::Color::blue(),
+                          2);
+        display.showText("tracked tag: 7",
+                         cmvr::common::Pixel{20, 30},
+                         cmvr::common::Color::white(),
+                         0.8,
+                         2);
+
+        const int key = display.show(30);
+        if (key == 27 || key == 'q' || key == 'Q') {
+            break;
+        }
+    }
+
+    display.close();
+}
+
+TEST(OpenCvImageCvDisplayTest, ShowTrackingImageWithActiveTagAndTargetPoint) {
+    const std::string serial = kRsSerial;
+    if (serial.empty()) {
+        GTEST_SKIP() << "kRsSerial is empty, please set it in image_display_test.cpp";
+    }
+
+    cmvr::config::RealSenseCameraConfig cam_cfg;
+    cam_cfg.set_id("image_display_test");
+    cam_cfg.set_serialnumber(serial);
+    cam_cfg.set_width(kRsWidth);
+    cam_cfg.set_height(kRsHeight);
+    cam_cfg.set_fps(kRsFps);
+    cam_cfg.set_codec("H265");
+    cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
+    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
+    cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
+    cam_cfg.set_buffer_size(30);
+    cam_cfg.set_sync(true);
+    cam_cfg.set_enable(true);
+
+    auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
+    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
+    perception->setTagSize(kTagSizeM);
+
+    cmvr::perception::TagRelativeTarget3D tracker(perception);
+    tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
+    tracker.setActiveTagSwitchPolicy(4, 1.2);
+    tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
+
+    cmvr::perception::AprilTagPerception::Options opt;
+    opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
+    opt.detect_tags = true;
+
+    ASSERT_NO_THROW(camera->init());
+    ASSERT_NO_THROW(camera->start());
+    struct CameraStopGuard {
+        std::shared_ptr<cmvr::device::RealsenseCamera> cam;
+        ~CameraStopGuard() {
+            if (!cam) return;
+            try {
+                cam->stop();
+            } catch (...) {
+            }
+        }
+    } stop_guard{camera};
+
+    cmvr::common::OpenCvImageCvDisplay display(kTrackingWindowName);
+    bool window_enabled = true;
+    try {
+        cv::namedWindow(kTrackingWindowName, cv::WINDOW_NORMAL);
+        cv::resizeWindow(kTrackingWindowName, kRsWidth, kRsHeight);
+        cv::imshow(kTrackingWindowName,
+                   cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20)));
+        cv::waitKey(1);
+    } catch (const cv::Exception& e) {
+        window_enabled = false;
+        std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
+    }
+    if (!window_enabled) {
+        GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
+    }
+
+    bool target_uv_initialized = false;
+    int target_u = kTargetU;
+    int target_v = kTargetV;
+    bool target_locked = false;
+
+    for (int frame = 0; frame < kDisplayFrames; ++frame) {
+        const bool update_ok = perception->update(opt);
+        const cv::Mat& color = perception->color();
+
+        cv::Mat display_image = color.empty()
+            ? cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20))
+            : color;
+        display.setImage(display_image);
+        display.clearOverlays();
+
+        if (!target_uv_initialized && !display_image.empty()) {
+            target_u = (kTargetU >= 0) ? kTargetU : (display_image.cols / 2);
+            target_v = (kTargetV >= 0) ? kTargetV : (display_image.rows / 2);
+            target_uv_initialized = true;
+        }
+
+        if (target_uv_initialized) {
+            const cmvr::common::Pixel selected_px{target_u, target_v};
+            display.showCross(selected_px, 10, cmvr::common::Color::yellow(), 2);
+            display.showCircle(selected_px, 14, cmvr::common::Color::yellow(), 2);
+        }
+
+        bool tracker_ok = false;
+        if (target_uv_initialized && !color.empty()) {
+            if (!target_locked) {
+                tracker_ok = tracker.startTrackingFromPixel(target_u, target_v);
+                target_locked = tracker_ok;
+            } else {
+                tracker_ok = tracker.track();
+            }
+        }
+
+        if (tracker_ok) {
+            cmvr::common::Pixel target_px{};
+            if (projectTargetToPixel(tracker.lastTargetInCamera(), perception->intrinsics(), target_px)) {
+                display.showPoint(target_px,
+                                  cmvr::common::Color::red(),
+                                  5,
+                                  -1,
+                                  "target");
+                display.showCircle(target_px, 18, cmvr::common::Color::red(), 2);
+            }
+        }
+
+        display.showText("active tag: " + std::to_string(tracker.activeTagId()),
+                         cmvr::common::Pixel{20, 30},
+                         cmvr::common::Color::white(),
+                         0.8,
+                         2);
+        display.showText("perception: " +
+                             std::string(cmvr::perception::AprilTagPerception::statusToString(
+                                 perception->lastStatus())),
+                         cmvr::common::Pixel{20, 60},
+                         cmvr::common::Color::white(),
+                         0.7,
+                         2);
+        display.showText("tracker: " +
+                             std::string(cmvr::perception::TagRelativeTarget3D::statusToString(
+                                 tracker.lastStatus())),
+                         cmvr::common::Pixel{20, 90},
+                         cmvr::common::Color::white(),
+                         0.7,
+                         2);
+        display.showText("tags: " + std::to_string(perception->tags().size()) +
+                             "  frame_ok: " + std::string(update_ok ? "true" : "false"),
+                         cmvr::common::Pixel{20, 120},
+                         cmvr::common::Color::white(),
+                         0.7,
+                         2);
+
+        if (tracker_ok) {
+            const Eigen::Vector3d& p_c_target = tracker.lastTargetInCamera();
+            display.showText("target_c: [" +
+                                 std::to_string(p_c_target.x()) + ", " +
+                                 std::to_string(p_c_target.y()) + ", " +
+                                 std::to_string(p_c_target.z()) + "]",
+                             cmvr::common::Pixel{20, 150},
+                             cmvr::common::Color::white(),
+                             0.6,
+                             2);
+        }
+
+        const int key = display.show(1);
+        if (key == 27 || key == 'q' || key == 'Q') {
+            break;
+        }
+    }
+
+    display.close();
+}

cmvr-es/controller/CMakeLists.txt

@@ -22,6 +22,7 @@ target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(controller PUBLIC
         protobuf
+        cmvr_es::perception
         cmvr_es::ik_solver
         cmvr_es::device::humanoid_robot
         gtest
@@ -70,6 +71,7 @@ add_executable(controller_test
 target_link_libraries(controller_test
         PRIVATE
         cmvr_es::utils
+        cmvr_es::perception
         cmvr_es::ik_solver
         cmvr_es::planner
         cmvr_es::proto

cmvr-es/controller/include/ibvs_controller.h

@@ -1,8 +1,6 @@
-//
-// Created by lgv on 2026/2/26.
-//
-
 #pragma once
+#ifndef CMVR_IBVS_CONTROLLER_H
+#define CMVR_IBVS_CONTROLLER_H
 
 #include <array>
 #include <memory>
@@ -13,52 +11,53 @@
 
 #include <visp3/core/vpHomogeneousMatrix.h>
 #include <visp3/core/vpPoint.h>
-#include <visp3/detection/vpDetectorAprilTag.h>
 #include <visp3/visual_features/vpFeaturePoint.h>
 #include <visp3/vs/vpServo.h>
 
-#include "devices/camera/abstract_camera.h"
 #include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "perception/include/apriltag_perception.h"
 
 namespace cmvr {
 
 /**
- * @brief 基于 AprilTag + ViSP + Pinocchio 的眼在手上 IBVS 控制器。
+ * @brief 基于 AprilTag 观测的眼在手上 IBVS 控制器。
+ *
+ * 坐标系约定：
+ * - `t`：当前被跟踪的 tag 坐标系。
+ * - `c`：ViSP 相机坐标系，也是视觉伺服任务中使用的相机坐标系。
+ * - `cam`：`AbstractCamera` 定义的相机坐标系。
+ * - `u`：URDF / IK 求解器使用的相机坐标系。
+ *
+ * 主要数据流：
+ * - `AprilTagPerception` 提供 tag 位姿 `cMo` / `T_c_t`；
+ * - 本类在相机坐标系 `c` 中构造视觉伺服误差；
+ * - 再将相机 twist 从 `c` 转到 `cam` 和 `u`，最后交给速度 IK。
  */
 class IbvsController {
 public:
-    /**
-     * @brief 深度使用模式。
-     */
     enum class DepthMode {
-        MONOCULAR = 0, /**< 仅使用 AprilTag 位姿估计得到的深度。 */
+        MONOCULAR = 0,
-        PREFER_DEPTH,  /**< 优先使用深度图；无效时回退到位姿深度。 */
+        PREFER_DEPTH,
-        DEPTH_ONLY     /**< 必须使用深度图；无效则本次计算失败。 */
+        DEPTH_ONLY
     };
 
-    /**
-     * @brief `compute()` 最近一次状态。
-     */
     enum class ComputeStatus {
-        OK = 0,        /**< 计算成功。 */
+        OK = 0,
-        NOT_READY,     /**< 控制器未初始化完成。 */
+        NOT_READY,
-        NO_NEW_FRAME,  /**< 未取到可用新帧。 */
+        NO_NEW_FRAME,
-        BAD_IMAGE,     /**< 图像格式或数据异常。 */
+        BAD_IMAGE,
-        INVALID_INPUT, /**< 输入参数异常。 */
+        INVALID_INPUT,
-        NO_DEPTH,      /**< 需要深度但深度不可用。 */
+        NO_DEPTH,
-        NO_TAG,        /**< 未检测到 AprilTag。 */
+        NO_TAG,
-        TAG_MISMATCH,  /**< 检测到 AprilTag，但与指定 id 不匹配。 */
+        TAG_MISMATCH,
-        IK_FAILED      /**< 速度 IK 求解失败。 */
+        IK_FAILED
     };
 
-    /**
-     * @brief 最近一次成功检测时使用的深度来源。
-     */
     enum class DepthUsage {
-        NONE = 0,  /**< 本帧未使用深度。 */
+        NONE = 0,
-        POSE_ONLY, /**< 使用位姿估计深度。 */
+        POSE_ONLY,
-        DEPTH_ONLY, /**< 使用深度图深度。 */
+        DEPTH_ONLY,
-        MIXED      /**< 混合使用（预留）。 */
+        MIXED
     };
 
     /**
@@ -67,68 +66,73 @@ public:
     IbvsController();
 
     /**
-     * @brief 初始化控制器与 DLS 速度 IK 求解器。
+     * @brief 初始化 DLS 速度 IK 求解器（不再需要相机）。
-     * @param camera 相机对象。
      * @param urdf_path URDF 文件路径。
-     * @param base_link 基坐标系 link 名称。
+     * @param base_link IK 链基座 link 名称。
-     * @param flange_link 法兰 link 名称。
+     * @param flange_link IK 链末端法兰 link 名称。
-     * @param camera_link 相机末端 link 名称（用于速度 IK）。
+     * @param camera_link URDF 中相机 link 名称，对应坐标系 `u`。
      * @return 初始化成功返回 `true`。
      */
-    bool init(const std::shared_ptr<device::AbstractCamera>& camera,
+    bool init(const std::string& urdf_path,
-              const std::string& urdf_path,
               const std::string& base_link,
               const std::string& flange_link,
               const std::string& camera_link);
 
+    /**
+     * @brief 注入共享 AprilTagPerception（必须设置；compute 依赖其缓存）。
+     *        会自动同步 tag_size 到控制模型（角点几何）。
+     * @param perception 共享的感知前端。
+     */
+    void setPerception(const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception);
+
+    const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception() const { return perception_; }
+
     /**
      * @brief 重置内部状态与关节命令缓存。
-     * @param q_init 初始关节命令；为空时清空缓存。
+     * @param q_init 初始关节位置命令；为空时清空内部缓存。
      */
     void reset(const std::vector<double>& q_init = {});
 
     /**
-     * @brief 根据当前图像与关节角计算下一拍关节位置命令。
+     * @brief 根据当前关节状态计算下一拍关节位置命令。
      * @param joints_angle 当前关节角。
-     * @param dt 控制周期（秒）。
+     * @param dt 控制周期，单位秒。
      * @param q_cmd_out 输出的下一拍关节位置命令。
      * @return 计算成功返回 `true`。
      */
     bool compute(const std::vector<double>& joints_angle,
                  double dt,
                  std::vector<double>& q_cmd_out);
+
     /**
-     * @brief 根据当前图像与关节角计算目标关节速度命令。
+     * @brief 根据当前关节状态计算关节速度命令。
      * @param joints_angle 当前关节角。
-     * @param qdot_out 输出的关节速度命令（rad/s）。
+     * @param qdot_out 输出的关节速度命令。
      * @return 计算成功返回 `true`。
      */
     bool compute(const std::vector<double>& joints_angle,
                  std::vector<double>& qdot_out);
 
     /**
-     * @brief 设置 ViSP 控制增益。
+     * @brief 设置 ViSP 视觉伺服增益。
-     * @param lambda 控制增益 `lambda`。
+     * @param lambda 视觉伺服增益。
      */
     void setLambda(double lambda);
+
     /**
-     * @brief 设置 AprilTag 边长。
+     * @brief 设置当前需要跟踪的 tag id。
-     * @param tag_size_m tag 边长（米）。
+     * @param tag_id 目标 tag id。
-     */
-    void setTagSize(double tag_size_m);
-    /**
-     * @brief 设置要跟踪的 tag id（必填）。
-     * @param tag_id 指定 tag id，必须 >= 0。
      */
     void setTrackedTagId(int tag_id);
+
     /**
      * @brief 设置期望目标位姿（`cMo_des tag 坐标系相对于VISP相机的期望位姿`）。
-     * @param x 期望平移 x（米）。
+     * @param x 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。
-     * @param y 期望平移 y（米）。
+     * @param y 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。
-     * @param z 期望平移 z（米）。
+     * @param z 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。
-     * @param rx 期望旋转参数 rx（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     * @param rx 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
-     * @param ry 期望旋转参数 ry（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     * @param ry 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
-     * @param rz 期望旋转参数 rz（弧度，直接传给 `vpRotationMatrix::buildFrom`）。
+     * @param rz 期望位姿 `cMo_des` 的旋转参数 rz，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
      */
     void setTarget(double x,
                    double y,
@@ -136,216 +140,264 @@ public:
                    double rx = 3.14159265358979323846,
                    double ry = 0.0,
                    double rz = 0.0);
+
     /**
-     * @brief 设置 DLS 阻尼。
+     * @brief 根据 tag 坐标系中的目标点，反算并设置发送给 setTarget() 的期望 tag 位姿。
+     *        设期望 tag 位姿为 cMo_des=(R_des, t_des)，则满足
+     *        p_c_target_des = R_des * p_t_target + t_des，
+     *        因而 t_des = p_c_target_des - R_des * p_t_target。
+     * @param p_t_target 目标点在当前被跟踪 tag 坐标系 t 中的坐标，单位米。
+     * @param p_c_target_des 目标点在 ViSP 相机坐标系 c 中的期望坐标，单位米。
+     * @param rx 期望位姿 cMo_des 的旋转参数 rx，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
+     * @param ry 期望位姿 cMo_des 的旋转参数 ry，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
+     * @param rz 期望位姿 cMo_des 的旋转参数 rz，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。
+     * @return 输入均为有限数并成功更新内部目标时返回 `true`，否则返回 `false`。
+     */
+    bool setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
+                                 const Eigen::Vector3d& p_c_target_des,
+                                 double rx = 3.14159265358979323846,
+                                 double ry = 0.0,
+                                 double rz = 0.0);
+
+    /**
+     * @brief 设置 DLS IK 的阻尼系数。
      * @param mu 阻尼系数。
      */
     void setMu(double mu);
+
     /**
-     * @brief 设置关节速度绝对值上限。
+     * @brief 设置每个关节速度命令的绝对值上限。
-     * @param qdot_max 关节最大速度（rad/s）。
+     * @param qdot_max 关节最大速度。
      */
     void setQdotMax(double qdot_max);
+
     /**
      * @brief 设置深度使用模式。
-     * @param mode 深度模式。
+     * @param mode 深度使用模式。
      */
     void setDepthMode(DepthMode mode);
+    DepthMode depthMode() const { return depth_mode_; }
+
     /**
      * @brief 设置深度闭环比例增益。
-     * @param kp 深度 `vz` 控制比例系数。
+     * @param kp 深度闭环比例系数。
      */
     void setDepthZGain(double kp);
+
     /**
      * @brief 设置相机 twist 六维限幅。
-     * @param vmax6 线速度/角速度六维限幅。
+     * @param vmax6 六维限幅，前三维为线速度，后三维为角速度。
      */
     void setVelocityLimit6(const std::array<double, 6>& vmax6);
+
     /**
-     * @brief 设置关节限位避障 null-space 项参数。
+     * @brief 设置关节限位回避参数。
-     *
      * @param enable 是否启用。
-     * @param gain 避障增益（rad/s 量纲）。
+     * @param gain 回避增益。
-     * @param margin_ratio 上下限触发边界占关节行程比例 (0, 0.5)。
+     * @param margin_ratio 触发限位回避的边界比例。
-     * @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。
+     * @param max_push 单关节最大推回速度。
      */
     void setJointLimitAvoidance(bool enable,
                                 double gain = 0.2,
                                 double margin_ratio = 0.05,
                                 double max_push = 0.25);
+
     /**
-     * @brief 设置 AbstractCamera 坐标系到 ViSP 坐标系旋转。
+     * @brief 设置从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。
      * @param R_cv 旋转矩阵。
      */
     void setAlignCameraToVisp(const Eigen::Matrix3d& R_cv);
+
     /**
-     * @brief 设置 AbstractCamera 坐标系到 URDF 相机坐标系旋转。
+     * @brief 设置从 `AbstractCamera` 相机坐标系 `cam` 到 URDF 相机坐标系 `u` 的旋转矩阵。
      * @param R_camera_urdf 旋转矩阵。
      */
     void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
 
-    /**
+    // 最近一帧是否检测到了当前被跟踪的 tag。
-     * @brief 最近一帧是否检测到“指定 id 的 tag”。
-     * @return 检测到返回 `true`。
-     */
     bool isTagDetected() const { return last_tag_detected_; }
-    /**
+
-     * @brief 获取最近一次计算状态。
+    // 最近一次 `compute()` 的状态。
-     * @return 计算状态。
-     */
     ComputeStatus lastComputeStatus() const { return last_compute_status_; }
-    /**
+
-     * @brief 计算状态转字符串。
+    // 计算状态转字符串。
-     * @param status 状态枚举。
-     * @return 状态字符串。
-     */
     static const char* statusToString(ComputeStatus status);
-    /**
+
-     * @brief 获取最近一帧深度来源。
+    // 最近一次 `compute()` 实际使用的深度来源。
-     * @return 深度来源枚举。
-     */
     DepthUsage lastDepthUsage() const { return last_depth_usage_; }
-    /**
+
-     * @brief 深度来源转字符串。
+    // 深度来源转字符串。
-     * @param usage 深度来源枚举。
-     * @return 深度来源字符串。
-     */
     static const char* depthUsageToString(DepthUsage usage);
-    /**
+
-     * @brief 获取最近检测到的 tag 平移（ViSP 相机坐标系）。
+    // 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
-     * @return tag 平移向量。
-     */
     const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
-    /**
+
-     * @brief 获取当前指定的 tag id。
+    // 当前配置要跟踪的 tag id。
-     * @return 指定的 tag id；若未设置返回 `-1`。
-     */
     int trackedTagId() const { return tracked_tag_id_; }
-    /**
+
-     * @brief 获取最近一次用于控制的 tag id。
+    // 最近一次成功控制时实际使用的 tag id。
-     * @return 最近使用的 tag id；若本帧未成功使用返回 `-1`。
-     */
     int lastUsedTagId() const { return last_used_tag_id_; }
-    /**
+
-     * @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。
+    // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
-     * @return 六维 twist 向量。
-     */
     const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
 
 private:
     /**
-     * @brief 核心计算链路：图像 -> 相机 twist -> 关节速度。
+     * @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。
      * @param joints_angle 当前关节角。
-     * @param qdot_out 输出的关节速度命令（rad/s）。
+     * @param qdot_out 输出的关节速度命令。
      * @return 计算成功返回 `true`。
      */
     bool computeInternal(const std::vector<double>& joints_angle,
                          std::vector<double>& qdot_out);
+
     /**
-     * @brief 按 URDF 关节上下限对关节命令做硬裁剪。
+     * @brief 按 URDF 关节上下限对关节命令做原地裁剪。
-     * @param q 关节命令，函数内原地修改。
+     * @param q 关节命令向量。
      */
     void clampJointCommandInPlace(std::vector<double>& q) const;
+
     /**
-     * @brief 根据当前目标位姿反解 tag 平面的深度控制点。
+     * @brief 从共享感知前端同步 tag 尺寸到控制模型。
+     * @param force 为 `true` 时即使尺寸未变化也强制重建任务。
+     */
+    void syncTagSizeFromPerception(bool force = false);
+
+    /**
+     * @brief 根据当前期望位姿 `cMo_des`，反算深度闭环控制点在 tag 坐标系 `t` 中的坐标。
      */
     void updateDepthControlPointInTag();
+
     /**
-     * @brief 重新构建 ViSP 任务与期望特征。
+     * @brief 按当前期望位姿和 tag 几何重新构建 ViSP 视觉伺服任务。
      */
     void initTask();
 
 private:
-    // 是否初始化成功。
+    // 控制器和 IK 求解器是否已成功初始化。
     bool initialized_{false};
-    // 速度 IK 使用的基座 frame 名称。
-    std::string base_frame_name_;
-    // 速度 IK 使用的末端相机 frame 名称。
-    std::string camera_frame_name_;
-    // 相机对象。
-    std::shared_ptr<device::AbstractCamera> camera_{nullptr};
 
-    // 参数
+    // IK 链基座 frame 名称。
-    // ViSP 控制增益。
+    std::string base_frame_name_;
+
+    // URDF 中相机 frame 名称，对应坐标系 `u`。
+    std::string camera_frame_name_;
+
+    // 共享感知前端。
+    std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
+
+    // ViSP 视觉伺服增益。
     double lambda_{0.7};
-    // tag 边长（米）。
+
+    // 控制模型使用的 tag 边长，单位米。
     double tag_size_m_{0.12};
-    // tag 半边长（米）。
+
+    // 控制模型使用的 tag 半边长，单位米。
     double tag_half_{0.06};
-    // 期望平移 x（米）。
+
+    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。
     double target_x_{0.0};
-    // 期望平移 y（米）。
+
+    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。
     double target_y_{0.0};
-    // 期望平移 z（米）。
+
+    // 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。
     double target_z_{0.33};
-    // 期望旋转参数 rx（弧度）。
+
+    // 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度。
     double target_rx_{3.14159265358979323846};
-    // 期望旋转参数 ry（弧度）。
+
+    // 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度。
     double target_ry_{0.0};
-    // 期望旋转参数 rz（弧度）。
+
+    // 期望位姿 `cMo_des` 的旋转参数 rz，单位弧度。
     double target_rz_{0.0};
-    // DLS 阻尼系数。
+
+    // DLS IK 阻尼系数。
     double mu_{0.02};
-    // 关节速度绝对值上限（rad/s）。
+
+    // 关节速度上限。
     double qdot_max_{0.6};
-    // 深度模式。
+
+    // 深度使用模式。
     DepthMode depth_mode_{DepthMode::MONOCULAR};
-    // 深度闭环增益：`vz = kp * (z_cur - target_z_)`。
+
+    // 深度闭环比例增益。
     double depth_z_kp_{1.0};
+
     // 相机 twist 六维限幅。
     std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
-    // 关节限位避障 null-space 参数。
+
+    // 是否启用关节限位回避。
     bool limit_avoidance_enabled_{false};
+
+    // 关节限位回避增益。
     double limit_avoidance_gain_{0.2};
+
+    // 关节限位回避触发边界比例。
     double limit_avoidance_margin_ratio_{0.15};
+
+    // 单关节最大推回速度。
     double limit_avoidance_max_push_{0.25};
-    // tag 平面中用于采样深度的控制点。
+
+    // 用于深度闭环的 tag 平面控制点，位于 tag 坐标系 `t` 的 xy 平面，单位米。
     Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
 
-    // 坐标对齐
+    // 从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。
-    // R = R_camera_urdf_ * （R_cv_转置）  == ViSP相机坐标系 到 URDF 相机系 的等效旋转
-    // AbstractCamera -> ViSP 的旋转矩阵。
     Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
-    // AbstractCamera -> URDF 相机系的旋转矩阵。
+
+    // 从 `AbstractCamera` 相机坐标系 `cam` 到 URDF 相机坐标系 `u` 的旋转矩阵。
     Eigen::Matrix3d R_camera_urdf_{Eigen::Matrix3d::Identity()};
 
-    // ViSP
+    // ViSP 视觉伺服任务。
-    // ViSP 伺服任务对象。
     std::unique_ptr<vpServo> task_{nullptr};
-    // tag 四角点（3D）。
+
+    // tag 四个角点在 tag 坐标系 `t` 中的 3D 模型点。
     vpPoint obj_pts_[4];
-    // 当前特征。
+
+    // 当前观测特征。
     vpFeaturePoint s_cur_[4];
-    // 目标特征。
+
+    // 期望特征。
     vpFeaturePoint s_star_[4];
-    // AprilTag 检测器。
+
-    vpDetectorAprilTag detector_;
+    // 当前配置要跟踪的 tag id。
-    // 指定跟踪的 tag id（必填）；<0 表示未设置。
     int tracked_tag_id_{-1};
 
     // 速度 IK 求解器。
     std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
-    // 是否成功读取到 URDF 关节限位。
+
+    // 是否成功读取 URDF 中的关节位置限位。
     bool has_joint_position_limits_{false};
-    // 本链关节位置下限（rad）。
+
+    // 关节位置下限。
     Eigen::VectorXd q_lower_limits_;
-    // 本链关节位置上限（rad）。
+
+    // 关节位置上限。
     Eigen::VectorXd q_upper_limits_;
-    // 最近一次用于控制的 tag id。
+
+    // 最近一次成功控制时实际使用的 tag id。
     int last_used_tag_id_{-1};
 
     // 内部积分得到的关节位置命令缓存。
     std::vector<double> q_cmd_;
-    // 最近一帧 tag 检测结果。
+
+    // 最近一帧是否检测到了当前被跟踪的 tag。
     bool last_tag_detected_{false};
-    // 最近一次 `compute()` 状态。
+
+    // 最近一次 `compute()` 的状态。
     ComputeStatus last_compute_status_{ComputeStatus::NOT_READY};
-    // 最近一帧深度来源。
+
+    // 最近一次 `compute()` 实际使用的深度来源。
     DepthUsage last_depth_usage_{DepthUsage::NONE};
-    // 最近一帧 tag 平移（ViSP 相机系）。
+
+    // 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
     Eigen::Vector3d last_tag_pos_visp_{Eigen::Vector3d::Zero()};
-    // 最近一帧相机 twist（ViSP 相机系）。
+
+    // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
     Eigen::Matrix<double, 6, 1> last_v_camera_visp_{Eigen::Matrix<double, 6, 1>::Zero()};
 };
 
 } // namespace cmvr
+
+#endif // CMVR_IBVS_CONTROLLER_H

cmvr-es/controller/src/controller_test.cpp

@@ -540,8 +540,9 @@ protected:
     ibvs_controller_->setDepthZGain(1.0);
     ibvs_controller_->setVelocityLimit6(vmax6_);
     ibvs_controller_->setTrackedTagId(tracked_tag_id_);
-    ibvs_controller_->setTarget(0.0,0.0,0.4);
+    ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.0, 0),
-   ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
+                                           Eigen::Vector3d(0.0, 0.0, 0.4));
+    ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
 
     const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
       1, 0, 0,
@@ -550,12 +551,19 @@ protected:
     ibvs_controller_->setAlignCameraToVisp(R_align);
     ibvs_controller_->setAlignCameraToUrdf(R_align);
 
-    if (!ibvs_controller_->init(mujoco_camera_, urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) {
+    if (!ibvs_controller_->init(urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) {
       std::cout << "[IBVS] IbvsController init failed" << std::endl;
       ready_ = false;
       return;
     }
 
+    perception_ = std::make_shared<cmvr::perception::AprilTagPerception>(mujoco_camera_);
+    perception_->setTagSize(tag_size_m_);
+    perception_opt_.detect_tags = true;
+    perception_opt_.fetch_encoded = false;
+    perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
+    ibvs_controller_->setPerception(perception_);
+
     std::vector<double> q_init(7, 0.0);
     for (int i = 0; i < 7; ++i) {
       q_init[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
@@ -609,6 +617,26 @@ protected:
       q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
     }
 
+    if (perception_) {
+      switch (ibvs_controller_->depthMode()) {
+        case IbvsController::DepthMode::MONOCULAR:
+          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
+          break;
+        case IbvsController::DepthMode::PREFER_DEPTH:
+          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER;
+          break;
+        case IbvsController::DepthMode::DEPTH_ONLY:
+          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
+          break;
+      }
+      const bool perception_ok = perception_->update(perception_opt_);
+      if (!perception_ok && (step_count_ % 60) == 0) {
+        std::cout << "[IBVS] perception update failed: "
+                  << cmvr::perception::AprilTagPerception::statusToString(perception_->lastStatus())
+                  << std::endl;
+      }
+    }
+
     std::vector<double> q_cmd_next;
     const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next);
 
@@ -617,6 +645,13 @@ protected:
         const auto st = ibvs_controller_->lastComputeStatus();
         std::cout << "[IBVS] compute skipped: "
                   << IbvsController::statusToString(st) << std::endl;
+        if (st == IbvsController::ComputeStatus::TAG_MISMATCH && perception_ && perception_->hasTags()) {
+          std::cout << "[IBVS] visible tag ids:";
+          for (const auto& t : perception_->tags()) {
+            std::cout << " " << t.id;
+          }
+          std::cout << std::endl;
+        }
       }
       ++step_count_;
       return;
@@ -698,9 +733,12 @@ private:
     "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
   std::string camera_frame_name_for_check_{"R_CAM"};
   int tracked_tag_id_{0};
+  double tag_size_m_{0.12};
 
   std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
   std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
+  std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
+  cmvr::perception::AprilTagPerception::Options perception_opt_{};
 
   std::array<double, 7> q_cmd_{{0, 0, 0, 0, 0, 0, 0}};
   int step_count_{0};

cmvr-es/controller/src/ibvs_controller.cpp

@@ -1,7 +1,3 @@
-//
-// Created by lgv on 2026/2/26.
-//
-
 #include "controller/include/ibvs_controller.h"
 
 #include "common/utils/image/image_process.h"
@@ -13,14 +9,11 @@
 #include <iostream>
 #include <limits>
 
-#include <opencv2/imgproc.hpp>
+#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpImage.h>
 
 namespace cmvr {
 
-
-
 const char* IbvsController::statusToString(ComputeStatus status) {
     switch (status) {
         case ComputeStatus::OK: return "ok";
@@ -46,39 +39,21 @@ const char* IbvsController::depthUsageToString(DepthUsage usage) {
     }
 }
 
-IbvsController::IbvsController()
+IbvsController::IbvsController() {
-    : detector_(vpDetectorAprilTag::TAG_36h11) {
+    R_cv_.setIdentity();
-    // AbstractCamera 相机系 -> ViSP 相机系
+    R_camera_urdf_.setIdentity();
-    R_cv_ = (Eigen::Matrix3d() <<
-        1, 0, 0,
-        0, 1, 0,
-        0, 0, 1).finished();
 
-    // AbstractCamera 相机系 -> URDF 相机系
-    R_camera_urdf_ = (Eigen::Matrix3d() <<
-        1, 0, 0,
-        0, 1, 0,
-        0, 0, 1).finished();
-
-    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
     updateDepthControlPointInTag();
     initTask();
 }
 
-bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
+bool IbvsController::init(const std::string& urdf_path,
-                          const std::string& urdf_path,
                           const std::string& base_link,
                           const std::string& flange_link,
                           const std::string& camera_link) {
-    camera_ = camera;
-    if (!camera_) {
-        initialized_ = false;
-        last_compute_status_ = ComputeStatus::NOT_READY;
-        return false;
-    }
-
     base_frame_name_ = base_link;
     camera_frame_name_ = camera_link;
+
     dls_solver_ = std::make_unique<PinocchioDlsIKSolver>(
         urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_);
 
@@ -95,10 +70,35 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
         q_lower_limits_.resize(0);
         q_upper_limits_.resize(0);
     }
+
     last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
     return initialized_;
 }
 
+void IbvsController::setPerception(const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception) {
+    perception_ = perception;
+    // 注入后立刻同步一次 tag size（用于控制模型）
+    syncTagSizeFromPerception(true);
+}
+
+void IbvsController::syncTagSizeFromPerception(bool force) {
+    if (!perception_) return;
+
+    const double s = perception_->tagSize();
+    if (!std::isfinite(s) || s <= 0.0) return;
+
+    if (!force && std::abs(s - tag_size_m_) < 1e-12) return;
+
+    tag_size_m_ = s;
+    tag_half_ = 0.5 * tag_size_m_;
+
+    // 深度控制点约束依赖 tag_half_
+    updateDepthControlPointInTag();
+
+    // 任务几何依赖 tag_half_
+    initTask();
+}
+
 void IbvsController::reset(const std::vector<double>& q_init) {
     if (q_init.empty()) {
         q_cmd_.clear();
@@ -106,6 +106,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
         q_cmd_ = q_init;
         clampJointCommandInPlace(q_cmd_);
     }
+
     last_tag_detected_ = false;
     last_used_tag_id_ = -1;
     last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
@@ -114,215 +115,6 @@ void IbvsController::reset(const std::vector<double>& q_init) {
     last_v_camera_visp_.setZero();
 }
 
-bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
-                                     std::vector<double>& qdot_out) {
-    last_depth_usage_ = DepthUsage::NONE;
-
-    if (!initialized_ || !camera_ || !dls_solver_) {
-        last_compute_status_ = ComputeStatus::NOT_READY;
-        return false;
-    }
-    if (joints_angle.empty()) {
-        last_compute_status_ = ComputeStatus::INVALID_INPUT;
-        return false;
-    }
-
-    cv::Mat color;
-    cv::Mat depth;
-    device::Rs2Intrinsics intrinsics{};
-    camera_->getRGBDImages(color, depth, intrinsics);
-
-    if (color.empty()) {
-        last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
-        return false;
-    }
-    if (color.channels() != 3) {
-        if (color.channels() != 1 && color.channels() != 4) {
-            last_compute_status_ = ComputeStatus::BAD_IMAGE;
-            return false;
-        }
-    }
-
-    const double fx = static_cast<double>(intrinsics.fx);
-    const double fy = static_cast<double>(intrinsics.fy);
-    const double cx = static_cast<double>(intrinsics.cx);
-    const double cy = static_cast<double>(intrinsics.cy);
-    if (fx <= 0.0 || fy <= 0.0) {
-        last_compute_status_ = ComputeStatus::INVALID_INPUT;
-        return false;
-    }
-
-    cv::Mat gray;
-    if (color.channels() == 1) {
-        gray = color;
-    } else if (color.channels() == 3) {
-        cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
-    } else {
-        cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
-    }
-    if (gray.empty() || gray.type() != CV_8UC1) {
-        last_compute_status_ = ComputeStatus::BAD_IMAGE;
-        return false;
-    }
-    if (!gray.isContinuous()) {
-        gray = gray.clone();
-    }
-
-    const int width = gray.cols;
-    const int height = gray.rows;
-    vpCameraParameters cam;
-    cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
-
-    vpImage<unsigned char> I(height, width);
-    for (int y = 0; y < height; ++y) {
-        std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
-    }
-
-    if (tracked_tag_id_ < 0) {
-        last_tag_detected_ = false;
-        last_used_tag_id_ = -1;
-        last_tag_pos_visp_.setZero();
-        last_compute_status_ = ComputeStatus::INVALID_INPUT;
-        return false;
-    }
-
-    std::vector<vpHomogeneousMatrix> cMo_vec;
-    const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
-    if (!detected || cMo_vec.empty()) {
-        last_tag_detected_ = false;
-        last_used_tag_id_ = -1;
-        last_tag_pos_visp_.setZero();
-        last_compute_status_ = ComputeStatus::NO_TAG;
-        return false;
-    }
-
-    // 只允许使用指定 id 的 tag，不再回退到“第一个检测结果”。
-    const std::vector<int> tag_ids = detector_.getTagsId();
-    const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
-    int selected_idx = -1;
-    for (size_t i = 0; i < pair_size; ++i) {
-        if (tag_ids[i] == tracked_tag_id_) {
-            selected_idx = static_cast<int>(i);
-            break;
-        }
-    }
-    if (selected_idx < 0) {
-        std::cout << "[IbvsController] TAG_MISMATCH target_tag_id=" << tracked_tag_id_
-                  << ", detected_tag_ids=[";
-        for (size_t i = 0; i < tag_ids.size(); ++i) {
-            if (i > 0) std::cout << ",";
-            std::cout << tag_ids[i];
-        }
-        std::cout << "]\n";
-
-        last_tag_detected_ = false;
-        last_used_tag_id_ = -1;
-        last_tag_pos_visp_.setZero();
-        last_compute_status_ = ComputeStatus::TAG_MISMATCH;
-        return false;
-    }
-
-    last_tag_detected_ = true;
-    last_used_tag_id_ = tracked_tag_id_;
-    vpHomogeneousMatrix cMo = cMo_vec[static_cast<size_t>(selected_idx)];
-    last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
-
-    // 深度控制点定义在 tag 平面（object frame）:
-    //   p_o = [x_t, y_t, 0, 1]^T
-    // 经位姿变换后在相机系:
-    //   p_c = cMo * p_o = [X, Y, Z, 1]^T
-    // vpPoint::get_x/get_y 给的是归一化坐标 x=X/Z, y=Y/Z。
-    vpPoint depth_ctrl_pt;
-    depth_ctrl_pt.setWorldCoordinates(depth_control_point_tag_.x(), depth_control_point_tag_.y(), 0.0);
-    depth_ctrl_pt.track(cMo);
-
-    const double x_depth_ctrl = depth_ctrl_pt.get_x();
-    const double y_depth_ctrl = depth_ctrl_pt.get_y();
-    double z_depth_ctrl = std::max(depth_ctrl_pt.get_Z(), 0.05);
-    bool depth_ctrl_used = false;
-
-    if (depth_mode_ != DepthMode::MONOCULAR) {
-        // 针孔投影:
-        //   u = fx * x + cx
-        //   v = fy * y + cy
-        const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
-        const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
-        double z_from_depth = 0.0;
-        if (ImageProcess::sampleDepthMeters(depth, u, v, z_from_depth)) {
-            z_depth_ctrl = std::max(z_from_depth, 0.05);
-            depth_ctrl_used = true;
-        } else if (depth_mode_ == DepthMode::DEPTH_ONLY) {
-            last_compute_status_ = ComputeStatus::NO_DEPTH;
-            return false;
-        }
-    }
-
-    for (int i = 0; i < 4; ++i) {
-        obj_pts_[i].track(cMo);
-        const double x = obj_pts_[i].get_x();
-        const double y = obj_pts_[i].get_y();
-        const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
-        s_cur_[i].buildFrom(x, y, Z);
-    }
-
-    if (depth_ctrl_used) {
-        last_depth_usage_ = DepthUsage::DEPTH_ONLY;
-    } else {
-        last_depth_usage_ = DepthUsage::POSE_ONLY;
-    }
-
-    vpColVector v_c = task_->computeControlLaw();
-
-    // 深度闭环（仅替换 z 方向）:
-    //   e_z = z_cur - z_target
-    //   v_z = k_p * e_z
-    // 其余 5 维仍沿用 ViSP IBVS 控制律输出。
-    if (depth_ctrl_used) {
-        v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
-    }
-
-    for (int i = 0; i < 6; ++i) {
-        v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
-        last_v_camera_visp_[i] = v_c[i];
-    }
-
-    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
-    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
-    // R_cv_: AbstractCamera -> ViSP，所以从 ViSP 回到 AbstractCamera 要乘转置:
-    //   v_cam = R_cv^T * v_visp
-    //   w_cam = R_cv^T * w_visp
-    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
-    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
-
-    Eigen::Matrix<double, 6, 1> twist_ee;
-    twist_ee << v_site(0), v_site(1), v_site(2), w_site(0), w_site(1), w_site(2);
-
-    Eigen::Matrix<double, 6, 1> twist_ee_pin;
-    // 线速度和角速度都用同一旋转做坐标变换:
-    //   v_urdf = R_camera_urdf * v_cam
-    //   w_urdf = R_camera_urdf * w_cam
-    twist_ee_pin.head<3>() = R_camera_urdf_ * twist_ee.head<3>();
-    twist_ee_pin.tail<3>() = R_camera_urdf_ * twist_ee.tail<3>();
-
-    dls_solver_->update_joints_state(joints_angle);
-    std::vector<double> qdot;
-    const bool ok = dls_solver_->ik(
-        base_frame_name_, camera_frame_name_, twist_ee_pin,
-        qdot, mu_, std::numeric_limits<double>::infinity());
-    if (!ok || qdot.size() != joints_angle.size()) {
-        last_compute_status_ = ComputeStatus::IK_FAILED;
-        return false;
-    }
-
-    qdot_out.resize(qdot.size());
-    for (size_t i = 0; i < qdot.size(); ++i) {
-        qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
-    }
-
-    last_compute_status_ = ComputeStatus::OK;
-    return true;
-}
-
 bool IbvsController::compute(const std::vector<double>& joints_angle,
                              double dt,
                              std::vector<double>& q_cmd_out) {
@@ -341,19 +133,10 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
     }
 
     for (size_t i = 0; i < qdot.size(); ++i) {
-        // 显式欧拉积分:
-        //   q_{k+1} = q_k + qdot * dt
         q_cmd_[i] += qdot[i] * dt;
     }
 
-
-    // std::cout << "b_cmd: " ;
-    // for (const auto& it : q_cmd_) {
-    //     std::cout << it << "  ";
-    // }
-    // std::cout << std::endl;
     clampJointCommandInPlace(q_cmd_);
-
     q_cmd_out = q_cmd_;
     return true;
 }
@@ -363,6 +146,153 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
     return computeInternal(joints_angle, qdot_out);
 }
 
+bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
+                                     std::vector<double>& qdot_out) {
+    last_depth_usage_ = DepthUsage::NONE;
+    last_tag_detected_ = false;
+    last_used_tag_id_ = -1;
+    last_tag_pos_visp_.setZero();
+    last_v_camera_visp_.setZero();
+
+    if (!initialized_ || !dls_solver_) {
+        last_compute_status_ = ComputeStatus::NOT_READY;
+        return false;
+    }
+    if (!perception_) {
+        last_compute_status_ = ComputeStatus::NOT_READY;
+        return false;
+    }
+    if (joints_angle.empty()) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+    if (tracked_tag_id_ < 0) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+
+    // 每帧确保 tag_size 同步（你可能运行时调 perception->setTagSize()）
+    syncTagSizeFromPerception(false);
+
+    // perception 必须先 update()，这里以 color 是否为空作为“是否ready”的简单判据
+    if (perception_->color().empty()) {
+        last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
+        return false;
+    }
+
+    if (!perception_->hasTags()) {
+        last_compute_status_ = ComputeStatus::NO_TAG;
+        return false;
+    }
+
+    const auto* tag = perception_->findTag(tracked_tag_id_);
+    if (!tag) {
+        last_compute_status_ = ComputeStatus::TAG_MISMATCH;
+        return false;
+    }
+
+    last_tag_detected_ = true;
+    last_used_tag_id_ = tracked_tag_id_;
+
+    const vpHomogeneousMatrix& cMo = tag->cMo;
+    last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
+
+    // intrinsics
+    const auto& intr = perception_->intrinsics();
+    const double fx = static_cast<double>(intr.fx);
+    const double fy = static_cast<double>(intr.fy);
+    const double cx = static_cast<double>(intr.cx);
+    const double cy = static_cast<double>(intr.cy);
+    if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
+        last_compute_status_ = ComputeStatus::INVALID_INPUT;
+        return false;
+    }
+
+    // 深度控制点（先用位姿 Z）
+    vpPoint depth_ctrl_pt;
+    depth_ctrl_pt.setWorldCoordinates(depth_control_point_tag_.x(), depth_control_point_tag_.y(), 0.0);
+    depth_ctrl_pt.track(cMo);
+
+    const double x_depth_ctrl = depth_ctrl_pt.get_x();
+    const double y_depth_ctrl = depth_ctrl_pt.get_y();
+    double z_depth_ctrl = std::max(depth_ctrl_pt.get_Z(), 0.05);
+    bool depth_ctrl_used = false;
+
+    const cv::Mat& depth = perception_->depth();
+    if (depth_mode_ != DepthMode::MONOCULAR) {
+        const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
+        const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
+
+        double z_from_depth = 0.0;
+        if (!depth.empty() && ImageProcess::sampleDepthMeters(depth, u, v, z_from_depth)) {
+            z_depth_ctrl = std::max(z_from_depth, 0.05);
+            depth_ctrl_used = true;
+        } else if (depth_mode_ == DepthMode::DEPTH_ONLY) {
+            last_compute_status_ = ComputeStatus::NO_DEPTH;
+            return false;
+        }
+    }
+
+    // 当前特征（四角点用 pose Z）
+    for (int i = 0; i < 4; ++i) {
+        obj_pts_[i].track(cMo);
+        const double x = obj_pts_[i].get_x();
+        const double y = obj_pts_[i].get_y();
+        const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
+        s_cur_[i].buildFrom(x, y, Z);
+    }
+
+    last_depth_usage_ = depth_ctrl_used ? DepthUsage::DEPTH_ONLY : DepthUsage::POSE_ONLY;
+
+    vpColVector v_c = task_->computeControlLaw();
+
+    // 深度闭环：仅替换 vz
+    if (depth_ctrl_used) {
+        v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
+    }
+
+    // 限幅+缓存（ViSP camera系）
+    for (int i = 0; i < 6; ++i) {
+        v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
+        last_v_camera_visp_[i] = v_c[i];
+    }
+
+    // ViSP -> AbstractCamera
+    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
+    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
+    Eigen::Vector3d v_cam = R_cv_.transpose() * v_visp;
+    Eigen::Vector3d w_cam = R_cv_.transpose() * w_visp;
+
+    Eigen::Matrix<double, 6, 1> twist_cam;
+    twist_cam << v_cam(0), v_cam(1), v_cam(2), w_cam(0), w_cam(1), w_cam(2);
+
+    // AbstractCamera -> URDF camera
+    Eigen::Matrix<double, 6, 1> twist_urdf;
+    twist_urdf.head<3>() = R_camera_urdf_ * twist_cam.head<3>();
+    twist_urdf.tail<3>() = R_camera_urdf_ * twist_cam.tail<3>();
+
+    // IK
+    dls_solver_->update_joints_state(joints_angle);
+
+    std::vector<double> qdot;
+    const bool ok = dls_solver_->ik(
+        base_frame_name_, camera_frame_name_, twist_urdf,
+        qdot, mu_, std::numeric_limits<double>::infinity());
+
+    if (!ok || qdot.size() != joints_angle.size()) {
+        last_compute_status_ = ComputeStatus::IK_FAILED;
+        return false;
+    }
+
+    qdot_out.resize(qdot.size());
+    for (size_t i = 0; i < qdot.size(); ++i) {
+        qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
+    }
+
+    last_compute_status_ = ComputeStatus::OK;
+    return true;
+}
+
 void IbvsController::clampJointCommandInPlace(std::vector<double>& q) const {
     if (!has_joint_position_limits_) return;
     if (q.size() != static_cast<size_t>(q_lower_limits_.size()) ||
@@ -382,12 +312,6 @@ void IbvsController::setLambda(double lambda) {
     initTask();
 }
 
-void IbvsController::setTagSize(double tag_size_m) {
-    tag_size_m_ = tag_size_m;
-    tag_half_ = tag_size_m_ * 0.5;
-    initTask();
-}
-
 void IbvsController::setTrackedTagId(int tag_id) {
     tracked_tag_id_ = tag_id;
 }
@@ -408,6 +332,39 @@ void IbvsController::setTarget(double x,
     initTask();
 }
 
+    bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
+                                                 const Eigen::Vector3d& p_c_target_des,
+                                                 double rx,
+                                                 double ry,
+                                                 double rz) {
+    if (!p_t_target.allFinite() || !p_c_target_des.allFinite()) {
+        return false;
+    }
+
+    vpRotationMatrix R_des_visp;
+    R_des_visp.buildFrom(rx, ry, rz);  // 注意：这是 rotvec（theta*u）
+
+    Eigen::Matrix3d R_des;
+    for (int r = 0; r < 3; ++r) {
+        for (int c = 0; c < 3; ++c) {
+            R_des(r, c) = R_des_visp[r][c];
+        }
+    }
+
+    if (!R_des.allFinite()) {
+        return false;
+    }
+
+    const Eigen::Vector3d t_des = p_c_target_des - R_des * p_t_target;
+
+    if (!t_des.allFinite()) {
+        return false;
+    }
+
+    setTarget(t_des.x(), t_des.y(), t_des.z(), rx, ry, rz);
+    return true;
+}
+
 void IbvsController::setMu(double mu) {
     mu_ = mu;
 }
@@ -436,6 +393,7 @@ void IbvsController::setJointLimitAvoidance(bool enable,
     limit_avoidance_gain_ = std::max(0.0, gain);
     limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
     limit_avoidance_max_push_ = max_push;
+
     if (dls_solver_) {
         dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
                                             limit_avoidance_gain_,
@@ -456,26 +414,19 @@ void IbvsController::updateDepthControlPointInTag() {
     vpRotationMatrix R_des;
     R_des.buildFrom(target_rx_, target_ry_, target_rz_);
 
-    // 反解深度控制点（tag 平面点）:
-    // 设控制点 p_o = [u, v, 0]^T，期望位姿为 (R_des, t_des)。
-    // 在相机系:
-    //   p_c = R_des * p_o + t_des
-    // 令控制点在期望时落在光轴上（x_c = 0, y_c = 0）:
-    //   [R00 R01] [u] = -[tx]
-    //   [R10 R11] [v]   [ty]
-    // 即 A * [u v]^T = b。
     Eigen::Matrix2d A;
     A << R_des[0][0], R_des[0][1],
          R_des[1][0], R_des[1][1];
     const Eigen::Vector2d b(-target_x_, -target_y_);
 
     if (std::abs(A.determinant()) < 1e-9) {
-        // 退化时回退到 tag 中心。
         depth_control_point_tag_.setZero();
         return;
     }
+
     depth_control_point_tag_ = A.fullPivLu().solve(b);
-    // 控制点限制在 tag 边界内，避免采样到背景。
+
+    // 限制在 tag 边界内
     depth_control_point_tag_.x() = SupportFunctions::clamp(depth_control_point_tag_.x(), -tag_half_, tag_half_);
     depth_control_point_tag_.y() = SupportFunctions::clamp(depth_control_point_tag_.y(), -tag_half_, tag_half_);
 }
@@ -487,14 +438,13 @@ void IbvsController::initTask() {
     task_->setLambda(lambda_);
 
     obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
-    obj_pts_[1].setWorldCoordinates(tag_half_, -tag_half_, 0.0);
+    obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0);
-    obj_pts_[2].setWorldCoordinates(tag_half_, tag_half_, 0.0);
+    obj_pts_[2].setWorldCoordinates( tag_half_,  tag_half_, 0.0);
-    obj_pts_[3].setWorldCoordinates(-tag_half_, tag_half_, 0.0);
+    obj_pts_[3].setWorldCoordinates(-tag_half_,  tag_half_, 0.0);
 
     vpTranslationVector t_des(target_x_, target_y_, target_z_);
     vpRotationMatrix R_des;
     R_des.buildFrom(target_rx_, target_ry_, target_rz_);
-    // cMo_des: 期望的 object(tag) 相对 camera 位姿。
     vpHomogeneousMatrix cMo_des(t_des, R_des);
 
     for (int i = 0; i < 4; ++i) {

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

@@ -306,7 +306,6 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     ibvs_controller.setLambda(0.6);
     ibvs_controller.setQdotMax(0.6);
     ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
-    ibvs_controller.setTagSize(0.12);
     ibvs_controller.setTrackedTagId(0);
     ibvs_controller.setTarget(0.0, 0.0, 0.40);
     ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
@@ -330,7 +329,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
 
 
 
-    ASSERT_TRUE(ibvs_controller.init(camera,
+    ASSERT_TRUE(ibvs_controller.init(
                                      "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
                                      "PELVIS_S",
                                      "R_WRIST_R_S",

cmvr-es/perception/CMakeLists.txt

@@ -3,6 +3,7 @@ find_package(OpenCV REQUIRED)
 
 add_library(perception SHARED
         src/tag_relative_target_3d.cpp
+        src/apriltag_perception.cpp
 )
 
 target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

cmvr-es/perception/include/apriltag_perception.h

@@ -0,0 +1,281 @@
+//
+// Created by lgv on 2026/3/5.
+//
+
+#pragma once
+#ifndef CMVR_PERCEPTION_APRILTAG_PERCEPTION_H
+#define CMVR_PERCEPTION_APRILTAG_PERCEPTION_H
+
+#include <cstdint>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include <Eigen/Dense>
+#include <opencv2/opencv.hpp>
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/detection/vpDetectorAprilTag.h>
+
+#include "devices/camera/abstract_camera.h"
+
+namespace cmvr::perception {
+
+/**
+ * @brief AprilTag 视觉前端：统一抓帧、缓存派生图像并输出 AprilTag 检测结果。
+ *
+ * 坐标系约定：
+ * - `c`：ViSP 相机坐标系，也是本类输出 `cMo` / `T_c_t` 所在的相机坐标系。
+ * - `t`：AprilTag 自身坐标系，原点位于 tag 中心，z 轴垂直于 tag 平面。
+ *
+ * 使用约定：
+ * - 每周期调用一次 `update()`；
+ * - 其他模块（如 `TagRelativeTarget3D` / `IbvsController`）只读取缓存，不重复抓帧。
+ */
+class AprilTagPerception {
+public:
+    enum class Status {
+        OK = 0,
+        NO_CAMERA,
+        NO_NEW_FRAME,
+        BAD_IMAGE,
+        INVALID_INTRINSICS,
+        NO_TAG,
+        NO_DEPTH,
+        ENCODED_UNAVAILABLE
+    };
+
+    enum class DepthPolicy {
+        // 只取 RGB。
+        NONE = 0,
+
+        // 优先取 RGBD；失败或无深度时回退到 RGB。
+        PREFER,
+
+        // 必须取 RGBD 且深度有效，否则本次更新失败。
+        REQUIRE
+    };
+
+    struct Options {
+        // 深度抓取策略。
+        DepthPolicy depth_policy{DepthPolicy::NONE};
+
+        // 是否执行 AprilTag 检测。
+        bool detect_tags{true};
+
+        // 是否抓取编码帧（StreamFrameData.rgbFrame / depthFrame）。
+        bool fetch_encoded{false};
+
+        // 抓取编码帧时传入相机驱动的索引。
+        size_t encoded_index{0};
+    };
+
+    struct Tag {
+        // AprilTag id。
+        int id{-1};
+
+        // AprilTag 检测的 decision margin，值越大通常表示检测越稳定。
+        double margin{1.0};
+
+        // ViSP 表示的 tag 位姿：tag 坐标系 `t` 相对于 ViSP 相机坐标系 `c` 的位姿。
+        vpHomogeneousMatrix cMo;
+
+        // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
+        Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
+    };
+
+    struct FrameCache {
+        // 原始流缓存，包含 RGB / depth / 编码帧 / 内参等信息。
+        device::StreamFrameData stream;
+
+        // 由 RGB 图转换得到的灰度图。
+        cv::Mat gray;
+
+        // 当前 `gray` 是否有效。
+        bool has_gray{false};
+
+        // ViSP 灰度图缓存，避免每帧重新分配。
+        vpImage<unsigned char> visp_I;
+
+        // 当前 `visp_I` 是否有效。
+        bool has_visp{false};
+
+        // `visp_I` 当前缓存的图像宽度。
+        int visp_w{0};
+
+        // `visp_I` 当前缓存的图像高度。
+        int visp_h{0};
+
+        // 本类内部维护的帧序号，每次 `update()` 成功后自增。
+        uint64_t frame_id{0};
+
+        void clearDerived() {
+            gray.release();
+            has_gray = false;
+            // visp_I 保留分配复用
+            has_visp = false;
+        }
+    };
+
+public:
+    /**
+     * @brief 构造感知前端。
+     * @param camera 相机对象；可为空，后续再通过 `setCamera()` 注入。
+     */
+    explicit AprilTagPerception(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
+
+    /**
+     * @brief 设置相机对象，并清空当前缓存与检测结果。
+     * @param camera 相机对象。
+     */
+    void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera);
+    const std::shared_ptr<cmvr::device::AbstractCamera>& camera() const { return camera_; }
+
+    /**
+     * @brief 设置 tag 物理边长。
+     * @param tag_size_m tag 边长，单位米。
+     */
+    void setTagSize(double tag_size_m);
+    double tagSize() const { return tag_size_m_; }
+
+    /**
+     * @brief 设置 AprilTag 家族。
+     * @param fam AprilTag 家族类型。
+     */
+    void setTagFamily(vpDetectorAprilTag::vpAprilTagFamily fam);
+
+    /**
+     * @brief 每周期调用一次：抓帧（RGB/RGBD，编码帧可选）+ detect（可选）+ 缓存结果。
+     * @return 成功返回 true。
+     */
+    bool update(const Options& opt);
+
+    /**
+     * @brief 获取最近一次 `update()` 的状态。
+     * @return 最近一次状态枚举。
+     */
+    Status lastStatus() const { return last_status_; }
+
+    /**
+     * @brief 状态枚举转字符串。
+     * @param s 状态枚举。
+     * @return 状态字符串。
+     */
+    static const char* statusToString(Status s);
+
+    /**
+     * @brief 获取当前缓存对应的帧序号。
+     * @return 帧序号。
+     */
+    uint64_t frameId() const { return frame_.frame_id; }
+
+    // ---- frame getters ----
+    // 完整帧缓存。
+    const FrameCache& frame() const { return frame_; }
+
+    // 原始流缓存。
+    const device::StreamFrameData& stream() const { return frame_.stream; }
+
+    // 当前 RGB 图。
+    const cv::Mat& color() const { return frame_.stream.rgbImage; }
+
+    // 当前深度图。
+    const cv::Mat& depth() const { return frame_.stream.depthImage; }
+
+    // 当前灰度图。
+    const cv::Mat& gray() const { return frame_.gray; }
+
+    // 当前相机内参。
+    const device::Rs2Intrinsics& intrinsics() const { return frame_.stream.intrinsics; }
+
+    // ViSP 相机模型参数。
+    const vpCameraParameters& vispCamera() const { return visp_cam_; }
+
+    // 当前缓存是否包含深度图。
+    bool hasDepth() const { return !frame_.stream.depthImage.empty(); }
+
+    // ---- detection outputs ----
+    // 当前缓存中是否包含至少一个检测到的 tag。
+    bool hasTags() const { return !tags_.empty(); }
+
+    // 当前帧的 tag 检测结果。
+    const std::vector<Tag>& tags() const { return tags_; }
+
+    /**
+     * @brief 按 id 查找当前帧检测到的 tag。
+     * @param id tag id。
+     * @return 找到则返回指针，否则返回 `nullptr`。
+     */
+    const Tag* findTag(int id) const;
+
+private:
+    /**
+     * @brief 按 `Options` 抓取当前帧的 RGB / depth 数据并填充 `frame_.stream`。
+     * @param opt 本次更新使用的抓帧选项。
+     * @return 抓帧成功返回 `true`。
+     */
+    bool grabFrame(const Options& opt);
+
+    /**
+     * @brief 抓取编码帧并写入 `frame_.stream`。
+     * @param encoded_index 相机驱动使用的编码帧索引。
+     * @return 抓取成功返回 `true`。
+     */
+    bool fetchEncoded(size_t encoded_index);
+
+    /**
+     * @brief 确保 `frame_.gray` 已由当前 RGB 图转换得到。
+     * @return 灰度图可用返回 `true`。
+     */
+    bool ensureGray();
+
+    /**
+     * @brief 确保 `frame_.visp_I` 已由当前灰度图填充。
+     * @return ViSP 灰度图可用返回 `true`。
+     */
+    bool ensureVispImage();
+
+    /**
+     * @brief 在当前帧上执行 AprilTag 检测，并填充 `tags_` / `id_to_index_`。
+     * @return 检测到至少一个 tag 返回 `true`。
+     */
+    bool detectTags();
+
+    /**
+     * @brief 将 ViSP 位姿 `cMo` 转为 Eigen 齐次变换 `T_c_t`。
+     * @param cMo tag 坐标系 `t` 相对于相机坐标系 `c` 的 ViSP 位姿。
+     * @return 对应的 Eigen 齐次变换 `T_c_t`。
+     */
+    static Eigen::Matrix4d toEigen4(const vpHomogeneousMatrix& cMo);
+
+private:
+    // 相机对象。
+    std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
+
+    // AprilTag 检测器。
+    vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
+
+    // tag 物理边长，单位米。
+    double tag_size_m_{0.12};
+
+    // 与当前内参对应的 ViSP 相机模型。
+    vpCameraParameters visp_cam_;
+
+    // 当前帧缓存。
+    FrameCache frame_;
+
+    // 当前帧检测到的 tag 列表。
+    std::vector<Tag> tags_;
+
+    // tag id 到 `tags_` 下标的映射。
+    std::unordered_map<int, size_t> id_to_index_;
+
+    // 最近一次 `update()` 的状态。
+    Status last_status_{Status::NO_NEW_FRAME};
+};
+
+} // namespace cmvr::perception
+
+#endif // CMVR_PERCEPTION_APRILTAG_PERCEPTION_H

cmvr-es/perception/include/tag_relative_target_3d.h

@@ -2,124 +2,98 @@
 #ifndef CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
 #define CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
 
+#include <cstddef>
+#include <cstdint>
+#include <limits>
 #include <memory>
 #include <unordered_map>
 #include <vector>
 
 #include <Eigen/Dense>
 #include <opencv2/opencv.hpp>
-#include <visp3/detection/vpDetectorAprilTag.h>
 
-#include "devices/camera/abstract_camera.h"
+#include "perception/include/apriltag_perception.h"
 
 namespace cmvr::perception {
 
 /**
- * @brief Tag 相对目标 3D 跟踪器（全封装版）。
+ * @brief 利用 AprilTag 观测跟踪一个目标点的 3D 位置。
  *
- * 该类内部完成：
+ * 坐标系约定：
- * 1) 从相机抓取 RGB 或 RGBD；
+ * - `c`：相机坐标系，与 `AprilTagPerception` 输出的 ViSP 相机坐标系一致。
- * 2) AprilTag 检测与位姿估计；
+ * - `t`：AprilTag 自身坐标系。
- * 3) 目标点像素 `(u, v)` 到相机 3D 点的求解；
+ * - `p_c_*`：点在相机坐标系 `c` 中的坐标。
- * 4) 目标点在 tag 坐标系下的锚点缓存与基于 active tag 的跟踪。
+ * - `p_t_*`：点在 tag 坐标系 `t` 中的坐标。
+ * - `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c` 的齐次变换。
  *
- * 对外仅暴露 `(u, v)` 输入，结果通过 `last*` getter 读取。
+ * 使用约定：
+ * - `AprilTagPerception` 每周期先调用一次 `update()`；
+ * - 本类只读取感知缓存，不主动抓帧或执行 AprilTag 检测；
+ * - public API 只传图像像素 `(u, v)`，几何结果通过 `last*` getter 读取。
  */
 class TagRelativeTarget3D {
 public:
-    /**
-     * @brief 最近一次调用的状态码。
-     */
     enum class Status {
-        OK = 0,            ///< 调用成功。
+        OK = 0,
-        INVALID_INPUT,     ///< 输入参数非法。
+        INVALID_INPUT,
-        NO_CAMERA,         ///< 相机对象为空。
+        NO_PERCEPTION,
-        NO_NEW_FRAME,      ///< 未获取到新图像帧。
+        PERCEPTION_NOT_READY,
-        BAD_IMAGE,         ///< 图像或内参无效。
+        NO_TAG,
-        NO_TAG,            ///< 未检测到 tag。
+        NO_DEPTH,
-        NO_OBSERVATION,    ///< 当前无可用 tag 观测。
+        BAD_IMAGE,
-        TARGET_NOT_LOCKED, ///< 尚未建立目标锚点。
+        NO_OBSERVATION,
-        NO_MATCHING_TAG    ///< 有观测但没有命中已缓存锚点。
+        TARGET_NOT_LOCKED,
+        NO_MATCHING_TAG
     };
 
-    /**
-     * @brief 目标像素转 3D 点的方法。
-     */
     enum class TargetPointMethod {
-        TAG_PLANE = 0, ///< 像素射线与 tag 平面求交（多 tag 融合）。
+        TAG_PLANE = 0, // 用像素射线与 tag 平面求交恢复目标点。
-        DEPTH_IMAGE    ///< 深度反投影（要求 RGBD 对齐）。
+        DEPTH_IMAGE    // 用深度图将像素反投影到相机坐标系。
     };
 
     /**
-     * @brief 构造函数。
+     * @brief 构造目标点跟踪器。
-     * @param camera 抽象相机对象，可为空。
+     * @param perception 共享的感知前端，可为空，后续再通过 `setPerception()` 注入。
      */
-    explicit TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
+    explicit TagRelativeTarget3D(const std::shared_ptr<AprilTagPerception>& perception = nullptr);
-    ~TagRelativeTarget3D() = default;
 
     /**
-     * @brief 设置/替换相机对象。
+     * @brief 设置共享感知前端。
-     * @param camera 抽象相机对象。
+     * @param perception 感知前端。
      */
-    void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera);
+    void setPerception(const std::shared_ptr<AprilTagPerception>& perception) { perception_ = perception; }
+    const std::shared_ptr<AprilTagPerception>& perception() const { return perception_; }
 
     /**
-     * @brief 清空内部状态（锚点、缓存、追踪状态与最近输出）。
+     * @brief 清空所有已锁定的目标锚点与跟踪状态。
      */
     void clear();
 
-    /**
+    // 当前是否还没有任何已锁定锚点。
-     * @brief 是否没有任何已缓存锚点。
-     * @return 没有锚点返回 `true`。
-     */
     bool empty() const { return anchors_in_tag_.empty(); }
 
-    /**
+    // 当前已保存的 tag 锚点数量。
-     * @brief 当前已缓存锚点数量。
-     * @return 锚点数量。
-     */
     size_t anchorCount() const { return anchors_in_tag_.size(); }
 
-    /**
+    // 最近一次接口调用的状态。
-     * @brief 获取最近一次调用状态。
-     * @return 状态码。
-     */
     Status lastStatus() const { return last_status_; }
 
-    /**
+    // 状态枚举转字符串。
-     * @brief 状态码转可读字符串。
-     * @param status 状态码。
-     * @return 对应字符串常量。
-     */
     static const char* statusToString(Status status);
 
     /**
-     * @brief 设置目标像素转 3D 方法。
+     * @brief 设置目标点恢复方式。
-     * @param method 求解方法。
+     * @param method 目标点恢复方式。
      */
     void setTargetPointMethod(TargetPointMethod method) { target_point_method_ = method; }
-
-    /**
-     * @brief 获取当前目标像素转 3D 方法。
-     * @return 当前方法。
-     */
     TargetPointMethod targetPointMethod() const { return target_point_method_; }
 
     /**
-     * @brief 设置 AprilTag 物理边长。
+     * @brief 设置深度采样配置，仅用于 `TargetPointMethod::DEPTH_IMAGE`。
-     * @param tag_size_m 边长（单位：米），必须大于 0。
+     * @param window_size_px 深度采样窗口边长，单位像素。
-     */
+     * @param enable_outlier_reject 是否启用深度离群点剔除。
-    void setTagSize(double tag_size_m);
+     * @param outlier_sigma 离群点剔除阈值，单位标准差倍数。
-
+     * @param outlier_min_dev_m 离群点剔除最小偏差，单位米。
-    /**
-     * @brief 设置深度采样与离群剔除参数。
-     *
-     * 该配置仅在 `DEPTH_IMAGE` 模式生效。
-     *
-     * @param window_size_px 深度采样窗口边长（像素，自动修正为奇数）。
-     * @param enable_outlier_reject 是否启用离群值剔除。
-     * @param outlier_sigma MAD 门限倍数。
-     * @param outlier_min_dev_m 离群剔除最小绝对阈值（米）。
      */
     void setDepthSamplingConfig(int window_size_px = 3,
                                 bool enable_outlier_reject = true,
@@ -127,9 +101,9 @@ public:
                                 double outlier_min_dev_m = 0.003);
 
     /**
-     * @brief 设置 active tag 切换策略。
+     * @brief 设置 active tag 自动切换策略。
-     * @param missing_before_switch active tag 连续丢失多少帧后允许切换。
+     * @param missing_before_switch 当前 active tag 连续丢失多少帧后允许切换。
-     * @param switch_hysteresis 新候选切换阈值倍率（>1 时更保守）。
+     * @param switch_hysteresis 候选 tag 相比 active tag 的切换迟滞系数。
      */
     void setActiveTagSwitchPolicy(int missing_before_switch = 4,
                                   double switch_hysteresis = 1.2);
@@ -137,240 +111,190 @@ public:
     /**
      * @brief 设置 active tag 候选评分权重。
      * @param weight_power 观测权重项指数。
-     * @param proximity_power 距离项指数（目标越靠近 tag 原点评分越高）。
+     * @param proximity_power 目标点在 tag 平面内邻近项指数。
-     * @param proximity_scale_m 距离归一化尺度（米）。
+     * @param proximity_scale_m 邻近项归一化尺度，单位米，位于 tag 坐标系 `t`。
      */
     void setTrackingCandidateScoreWeights(double weight_power = 1.0,
                                           double proximity_power = 1.0,
                                           double proximity_scale_m = 0.08);
 
     /**
-     * @brief 重置 active tag 跟踪状态（不清空锚点）。
+     * @brief 清空当前 active tag 选择状态，但保留已锁定锚点。
      */
     void resetActiveTagTracking();
 
-    /**
+    // 当前 active tag 的 id；未选中时为 -1。
-     * @brief 当前 active tag id。
-     * @return active tag id；无效时为 `-1`。
-     */
     int activeTagId() const { return active_tag_id_; }
 
-    /**
+    // 当前 active tag 已连续丢失的帧数。
-     * @brief 当前 active tag 连续丢失帧数。
-     * @return 丢失帧计数。
-     */
     int activeTagMissingCount() const { return active_tag_missing_count_; }
 
-
     /**
-     * @brief 对目标像素进行单次 3D 解算。
+     * @brief 对单个像素 `(u, v)` 执行一次目标点恢复。
-     * @param u 像素列坐标。
+     * @param u 图像列坐标，单位像素。
-     * @param v 像素行坐标。
+     * @param v 图像行坐标，单位像素。
-     * @return 成功返回 `true`，结果写入 `last*`。
+     * @return 成功时返回 `true`，结果写入 `lastTargetInCamera()` 等 getter。
      */
     bool solveFromPixel(int u, int v);
 
     /**
-     * @brief 以目标像素初始化跟踪（解算 + 建锚点 + 选择 active tag）。
+     * @brief 根据像素 `(u, v)` 锁定目标点，并在当前可见 tag 上建立锚点。
-     * @param u 像素列坐标。
+     * @param u 图像列坐标，单位像素。
-     * @param v 像素行坐标。
+     * @param v 图像行坐标，单位像素。
-     * @return 成功返回 `true`，结果写入 `last*`。
+     * @return 成功时返回 `true`，并初始化 active tag。
      */
     bool startTrackingFromPixel(int u, int v);
 
     /**
-     * @brief 进行一次跟踪迭代（active tag 跟踪 + 必要时自动切换）。
+     * @brief 使用当前缓存的 tag 观测跟踪已锁定目标点。
-     * @return 成功返回 `true`，结果写入 `last*`。
+     * @return 成功时返回 `true`，结果写入 `lastTargetInCamera()` 等 getter。
      */
     bool track();
 
-    /**
+    // 最近一次解算/跟踪得到的目标点在相机坐标系 `c` 中的位置。
-     * @brief 最近一次成功解算/跟踪得到的目标点相机坐标。
-     * @return 目标点 `p_c_target`（米）。
-     */
     const Eigen::Vector3d& lastTargetInCamera() const { return last_p_c_target_; }
 
-    /**
+    // 最近一次结果实际使用的 tag id；深度锁点时通常为 -1。
-     * @brief 最近一次结果对应的主导 tag id。
-     * @return 平面法为主导 tag id；深度法通常为 `-1`。
-     */
     int lastUsedTagId() const { return last_used_tag_id_; }
 
-    /**
+    // 最近一次多 tag 融合结果的离散度，单位米。
-     * @brief 最近一次平面法融合离散度。
-     * @return 离散度（米）；深度法通常为 0。
-     */
     double lastSpread() const { return last_spread_m_; }
 
-    /**
+    // 最近一次 `track()` 是否发生了 active tag 切换。
-     * @brief 最近一次 `track()` 是否发生 active tag 切换。
-     * @return 切换返回 `true`。
-     */
     bool lastSwitched() const { return last_switched_; }
 
     /**
-     * @brief 是否存在指定 tag 的目标锚点。
+     * @brief 判断某个 tag 是否已经保存了目标点锚点。
      * @param tag_id tag id。
-     * @return 存在返回 `true`。
+     * @return 已保存则返回 `true`。
      */
     bool hasAnchorForTag(int tag_id) const;
 
     /**
-     * @brief 获取指定 tag 坐标系下缓存的目标点坐标。
+     * @brief 获取目标点在指定 tag 坐标系 `t` 中的锚点坐标。
      * @param tag_id tag id。
-     * @param p_t_target_out 输出目标点在该 tag 坐标系下的坐标。
+     * @param p_t_target_out 输出的目标点坐标，位于 tag 坐标系 `t`，单位米。
-     * @return 成功返回 `true`。
+     * @return 找到锚点则返回 `true`。
      */
     bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const;
 
 private:
-    /**
+    struct Observation {
-     * @brief 单帧 tag 位姿观测（tag 坐标系到相机坐标系）。
+        // 当前观测对应的 tag id。
-     */
-    struct TagPoseObservation {
-        // tag id。
         int tag_id{-1};
-        // tag -> camera 变换矩阵。
-        Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
-        // 观测权重（检测置信度）。
-        double weight{1.0};
-    };
 
-    /**
+        // 当前观测的 `T_c_t`，将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
-     * @brief 最近一帧图像与内参缓存。
+        Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
-     */
+
-    struct FrameCache {
+        // 当前观测的权重，通常来自 tag 检测质量。
-        // RGB 图。
+        double weight{1.0};
-        cv::Mat color;
-        // 深度图。
-        cv::Mat depth;
-        // 相机内参。
-        device::Rs2Intrinsics intr{};
-        // 当前缓存是否有 RGB。
-        bool has_color{false};
-        // 当前缓存是否有深度。
-        bool has_depth{false};
-        // 当前缓存是否有内参。
-        bool has_intr{false};
-        // 帧序号（用于观测同步）。
-        uint64_t frame_id{0};
     };
 
 private:
     /**
-     * @brief 重置最近一次调用输出缓存。
+     * @brief 清空最近一次解算/跟踪输出缓存。
      */
     void resetLastOutputs();
 
     /**
-     * @brief 内部抓帧与观测更新入口。
+     * @brief 从 `perception_` 同步当前帧缓存。
-     * @param need_depth 是否需要深度图。
+     * @param need_tags 是否要求当前帧必须含有 tag 观测。
-     * @param need_tags 是否需要更新 tag 观测。
+     * @param need_depth 是否要求当前帧必须含有深度图。
-     * @return 成功返回 `true`。
+     * @return 同步成功且满足要求返回 `true`。
      */
-    bool updateInternal(bool need_depth, bool need_tags);
+    bool syncFromPerception(bool need_tags, bool need_depth);
 
     /**
-     * @brief 抓取一帧 RGB 与内参。
+     * @brief 用像素射线与当前可见 tag 平面求交，恢复目标点在相机坐标系 `c` 中的位置。
-     * @return 成功返回 `true`。
+     * @param u 图像列坐标，单位像素。
+     * @param v 图像行坐标，单位像素。
+     * @return 恢复成功返回 `true`。
      */
-    bool grabRGB();
+    bool solveFromPixelOnPlane(int u, int v);
 
     /**
-     * @brief 抓取一帧 RGBD 与内参。
+     * @brief 用深度图将像素 `(u, v)` 反投影到相机坐标系 `c`。
-     * @return 成功返回 `true`。
+     * @param u 图像列坐标，单位像素。
+     * @param v 图像行坐标，单位像素。
+     * @return 反投影成功返回 `true`。
      */
-    bool grabRGBD();
+    bool solveFromPixelWithDepth(int u, int v);
 
     /**
-     * @brief 从当前缓存 RGB + 内参中检测 tag 并更新 `observations_`。
+     * @brief 用目标点在相机坐标系 `c` 中的位置，给当前可见 tag 建立或补充锚点。
-     * @return 成功返回 `true`。
+     * @param p_c_target 目标点在相机坐标系 `c` 中的位置，单位米。
+     * @param overwrite_existing 是否覆盖已有的 `p_t_target` 锚点。
+     * @return 建锚成功返回 `true`。
      */
-    bool updateObservations(); // uses frame_.color + frame_.intr
+    bool lockAnchorsFromPoint(const Eigen::Vector3d& p_c_target, bool overwrite_existing);
 
     /**
-     * @brief 依据当前观测将目标点写入各 tag 锚点缓存。
+     * @brief 由已存在的锚点恢复当前帧目标点在相机坐标系 `c` 中的位置。
-     * @param p_c_target 目标点在相机坐标系下位置。
+     * @param multi_fuse 为 `true` 时融合所有可见且已建锚点的 tag；否则只使用 active tag。
-     * @param overwrite_existing 是否覆盖已有锚点。
+     * @return 恢复成功返回 `true`。
-     * @return 成功返回 `true`。
      */
-    bool lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing);
+    bool resolveFromAnchors(bool multi_fuse);
 
     /**
-     * @brief 依据当前观测与锚点恢复目标点相机坐标。
+     * @brief 根据锁定时求得的目标点初始化锚点和 active tag。
-     * @return 成功返回 `true`。
+     * @param p_c_lock 锁定时目标点在相机坐标系 `c` 中的位置，单位米。
-     */
+     * @param used_tag 锁定时实际使用的 tag id。
-    bool resolveTargetInCameraInternal(); // anchors + observations_ => last_p_c_target_ (+spread/+used)
+     * @param spread 锁定时多 tag 融合离散度，单位米。
-
+     * @return 初始化成功返回 `true`。
-    /**
-     * @brief 在当前缓存下使用平面法解像素目标点。
-     * @param u 像素列坐标。
-     * @param v 像素行坐标。
-     * @return 成功返回 `true`。
-     */
-    bool solveFromPixelOnPlaneCached(int u, int v);
-
-    /**
-     * @brief 在当前缓存下使用深度法解像素目标点。
-     * @param u 像素列坐标。
-     * @param v 像素行坐标。
-     * @return 成功返回 `true`。
-     */
-    bool solveFromPixelWithDepthCached(int u, int v);
-
-    /**
-     * @brief 使用锁定点初始化锚点并选择 active tag。
-     * @param p_c_lock 锁定时目标点相机坐标。
-     * @param used_tag 平面法主导 tag id（深度法可为 -1）。
-     * @param spread 平面法融合离散度。
-     * @return 成功返回 `true`。
      */
     bool initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock,
                                        int used_tag,
                                        double spread);
 
     /**
-     * @brief 在当前缓存观测下执行 active tag 跟踪。
+     * @brief 使用当前 active tag 跟踪目标点，并在需要时切换 active tag。
-     * @return 成功返回 `true`。
+     * @return 跟踪成功返回 `true`。
      */
-    bool trackWithActiveTagFromCached();
+    bool trackWithActiveTag();
 
     /**
-     * @brief 判断 4x4 矩阵元素是否全为有限数。
+     * @brief 计算某个观测作为 active tag 候选时的评分。
-     * @param T 输入矩阵。
+     * @param obs 当前帧某个 tag 的观测。
-     * @return 全有限返回 `true`。
+     * @return 候选评分，越大越优。
+     */
+    double trackingCandidateScore(const Observation& obs) const;
+
+    /**
+     * @brief 判断齐次变换矩阵中的所有元素是否为有限数。
+     * @param T 齐次变换矩阵。
+     * @return 全部为有限数返回 `true`。
      */
     static bool isFiniteMatrix(const Eigen::Matrix4d& T);
 
     /**
-     * @brief 判断 3D 向量是否全为有限数。
+     * @brief 判断三维点坐标是否为有限数。
-     * @param p 输入向量。
+     * @param p 三维点。
-     * @return 全有限返回 `true`。
+     * @return 全部为有限数返回 `true`。
      */
     static bool isFiniteVector(const Eigen::Vector3d& p);
 
     /**
-     * @brief 点从 tag 坐标系变换到相机坐标系。
+     * @brief 将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
-     * @param T_c_t tag 到相机变换。
+     * @param T_c_t 齐次变换，表示 `t -> c`。
-     * @param p_t tag 坐标系下点。
+     * @param p_t 点在 tag 坐标系 `t` 中的坐标，单位米。
-     * @return 相机坐标系下点。
+     * @return 点在相机坐标系 `c` 中的坐标，单位米。
      */
     static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t);
 
     /**
-     * @brief 点从相机坐标系变换到 tag 坐标系。
+     * @brief 将相机坐标系 `c` 中的点变换到 tag 坐标系 `t`。
-     * @param T_c_t tag 到相机变换。
+     * @param T_c_t 齐次变换，表示 `t -> c`。
-     * @param p_c 相机坐标系下点。
+     * @param p_c 点在相机坐标系 `c` 中的坐标，单位米。
-     * @return tag 坐标系下点。
+     * @return 点在 tag 坐标系 `t` 中的坐标，单位米。
      */
     static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c);
 
     /**
-     * @brief 相机射线与 tag 平面求交。
+     * @brief 计算相机像素射线与 tag 平面的交点。
-     * @param T_c_t tag 到相机变换。
+     * @param T_c_t 齐次变换，表示 `t -> c`。
-     * @param ray_c 相机坐标系下射线方向。
+     * @param ray_c 像素射线方向，位于相机坐标系 `c`。
-     * @param p_c_intersection 输出交点（相机坐标系）。
+     * @param p_c_intersection 输出交点，位于相机坐标系 `c`，单位米。
-     * @param view_cos_out 可选输出：视线与平面法向夹角余弦绝对值。
+     * @param view_cos_out 输出射线与 tag 法向夹角余弦的绝对值，可为空。
      * @return 求交成功返回 `true`。
      */
     static bool intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
@@ -379,76 +303,80 @@ private:
                                          double* view_cos_out = nullptr);
 
     /**
-     * @brief 归一化观测权重（非法权重回退到默认值）。
+     * @brief 将观测权重清洗为合法的正数。
-     * @param w 输入权重。
+     * @param w 原始权重。
-     * @return 处理后的权重。
+     * @return 清洗后的权重。
      */
-    static double sanitizeObsWeight(double w);
+    static double sanitizeWeight(double w);
-
-    /**
-     * @brief 计算单个观测作为 active tag 候选的评分。
-     * @param obs tag 观测。
-     * @return 候选评分。
-     */
-    double trackingCandidateScore(const TagPoseObservation& obs) const;
 
 private:
-    // 目标锚点缓存：tag_id -> p_t_target。
+    // 共享感知前端。
+    std::shared_ptr<AprilTagPerception> perception_{nullptr};
+
+    // 当前使用的感知帧 id。
+    uint64_t used_frame_id_{0};
+
+    // 当前帧相机内参。
+    device::Rs2Intrinsics intr_{};
+
+    // 指向当前帧深度图的只读引用。
+    const cv::Mat* depth_{nullptr};
+
+    // 当前帧 tag 观测列表。
+    std::vector<Observation> observations_;
+
+    // 目标点锚点表：`tag_id -> p_t_target`，其中 `p_t_target` 位于对应 tag 坐标系 `t`。
     std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_;
 
-    // 相机对象。
+    // 最近一次接口调用的状态。
-    std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
-
-    // AprilTag 检测器。
-    vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
-    // AprilTag 物理边长（米）。
-    double tag_size_m_{0.12};
-
-    // 最近一次调用状态。
     Status last_status_{Status::TARGET_NOT_LOCKED};
 
-    // 图像/内参缓存。
-    FrameCache frame_;
-    // 当前帧 tag 观测缓存。
-    std::vector<TagPoseObservation> observations_;
-    // 观测缓存对应的帧 id。
-    uint64_t obs_frame_id_{0};
-
     // 当前 active tag id。
     int active_tag_id_{-1};
-    // active tag 连续丢失帧计数。
+
+    // 当前 active tag 连续丢失的帧数。
     int active_tag_missing_count_{0};
-    // 连续丢失多少帧后允许切换 active tag。
+
+    // active tag 丢失多少帧后允许切换。
     int missing_before_switch_{4};
-    // active 切换迟滞阈值。
+
+    // active tag 切换迟滞系数。
     double switch_hysteresis_{1.2};
 
-    // 候选评分中权重项指数。
+    // 候选评分中的观测权重指数。
     double candidate_weight_power_{1.0};
-    // 候选评分中距离项指数。
+
+    // 候选评分中的邻近项指数。
     double candidate_proximity_power_{1.0};
-    // 候选评分距离归一化尺度（米）。
+
+    // 候选评分中的邻近项归一化尺度，单位米，位于 tag 坐标系 `t`。
     double candidate_proximity_scale_m_{0.08};
 
-    // 当前像素转 3D 方法。
+    // 目标点恢复方式。
     TargetPointMethod target_point_method_{TargetPointMethod::TAG_PLANE};
 
-    // 深度采样窗口边长（像素）。
+    // 深度采样窗口边长，单位像素。
     int depth_window_size_px_{3};
-    // 是否启用深度离群剔除。
+
+    // 是否启用深度离群点剔除。
     bool depth_outlier_reject_enabled_{true};
-    // 深度离群剔除 sigma 参数。
+
+    // 深度离群点剔除阈值，单位标准差倍数。
     double depth_outlier_sigma_{2.5};
-    // 深度离群剔除最小绝对偏差（米）。
+
+    // 深度离群点剔除最小偏差，单位米。
     double depth_outlier_min_dev_m_{0.002};
 
-    // 最近一次输出的目标点相机坐标。
+    // 最近一次解算/跟踪得到的目标点，位于相机坐标系 `c`。
     Eigen::Vector3d last_p_c_target_{Eigen::Vector3d::Zero()};
-    // 最近一次输出使用的主导 tag id。
+
+    // 最近一次结果实际使用的 tag id。
     int last_used_tag_id_{-1};
-    // 最近一次输出离散度（米）。
+
+    // 最近一次多 tag 融合结果的离散度，单位米。
     double last_spread_m_{0.0};
-    // 最近一次 track 是否发生 active tag 切换。
+
+    // 最近一次 `track()` 是否发生了 active tag 切换。
     bool last_switched_{false};
 };
 

cmvr-es/perception/src/apriltag_perception.cpp

@@ -0,0 +1,286 @@
+//
+// Created by lgv on 2026/3/5.
+//
+
+#include "perception/include/apriltag_perception.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+#include <opencv2/imgproc.hpp>
+
+namespace cmvr::perception {
+
+AprilTagPerception::AprilTagPerception(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
+    : camera_(camera) {
+    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
+}
+
+void AprilTagPerception::setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) {
+    camera_ = camera;
+    tags_.clear();
+    id_to_index_.clear();
+    frame_.stream = device::StreamFrameData{};
+    frame_.clearDerived();
+    frame_.frame_id = 0;
+    last_status_ = Status::NO_NEW_FRAME;
+}
+
+void AprilTagPerception::setTagSize(double tag_size_m) {
+    if (std::isfinite(tag_size_m) && tag_size_m > 0.0) {
+        tag_size_m_ = tag_size_m;
+    }
+}
+
+void AprilTagPerception::setTagFamily(vpDetectorAprilTag::vpAprilTagFamily fam) {
+    detector_ = vpDetectorAprilTag(fam);
+    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
+}
+
+const char* AprilTagPerception::statusToString(Status s) {
+    switch (s) {
+        case Status::OK: return "ok";
+        case Status::NO_CAMERA: return "no_camera";
+        case Status::NO_NEW_FRAME: return "no_new_frame";
+        case Status::BAD_IMAGE: return "bad_image";
+        case Status::INVALID_INTRINSICS: return "invalid_intrinsics";
+        case Status::NO_TAG: return "no_tag";
+        case Status::NO_DEPTH: return "no_depth";
+        case Status::ENCODED_UNAVAILABLE: return "encoded_unavailable";
+        default: return "unknown";
+    }
+}
+
+bool AprilTagPerception::update(const Options& opt) {
+    tags_.clear();
+    id_to_index_.clear();
+    frame_.clearDerived();
+
+    if (!grabFrame(opt)) {
+        return false;
+    }
+
+    // intrinsics -> visp camera params
+    const double fx = static_cast<double>(frame_.stream.intrinsics.fx);
+    const double fy = static_cast<double>(frame_.stream.intrinsics.fy);
+    const double cx = static_cast<double>(frame_.stream.intrinsics.cx);
+    const double cy = static_cast<double>(frame_.stream.intrinsics.cy);
+
+    if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
+        last_status_ = Status::INVALID_INTRINSICS;
+        return false;
+    }
+    visp_cam_.initPersProjWithoutDistortion(fx, fy, cx, cy);
+
+    // optional: fetch encoded (after we have the newest frame)
+    if (opt.fetch_encoded) {
+        if (!fetchEncoded(opt.encoded_index)) {
+            // 编码帧抓取失败不一定要致命（看你喜好）
+            // 这里按“失败但继续 detect/输出 Mat”处理，只设置状态供外部看
+            last_status_ = Status::ENCODED_UNAVAILABLE;
+            // 注意：不 return false，让 detect 还能跑；你也可以改成 return false
+        }
+    }
+
+    // detect tags
+    if (opt.detect_tags) {
+        if (!ensureVispImage()) return false;
+        if (!detectTags()) return false;
+    } else {
+        last_status_ = Status::OK;
+    }
+
+    frame_.frame_id++;
+    return (last_status_ == Status::OK);
+}
+
+bool AprilTagPerception::grabFrame(const Options& opt) {
+    if (!camera_) {
+        last_status_ = Status::NO_CAMERA;
+        return false;
+    }
+
+    // 清空 stream 中 Mat，但保留结构体可复用
+    frame_.stream.rgbImage.release();
+    frame_.stream.depthImage.release();
+    frame_.stream.rgbFrame.clear();
+    frame_.stream.depthFrame.clear();
+    frame_.stream.bKey = false;
+    frame_.stream.depthKey = false;
+
+    try {
+        if (opt.depth_policy == DepthPolicy::NONE) {
+            camera_->getRGBImage(frame_.stream.rgbImage, frame_.stream.intrinsics);
+        } else {
+            camera_->getRGBDImages(frame_.stream.rgbImage, frame_.stream.depthImage, frame_.stream.intrinsics);
+        }
+    } catch (...) {
+        // PREFER: 允许回退 RGB
+        if (opt.depth_policy == DepthPolicy::PREFER) {
+            try {
+                camera_->getRGBImage(frame_.stream.rgbImage, frame_.stream.intrinsics);
+                frame_.stream.depthImage.release();
+            } catch (...) {
+                last_status_ = Status::NO_NEW_FRAME;
+                return false;
+            }
+        } else {
+            last_status_ = Status::NO_NEW_FRAME;
+            return false;
+        }
+    }
+
+    if (frame_.stream.rgbImage.empty()) {
+        last_status_ = Status::NO_NEW_FRAME;
+        return false;
+    }
+
+    // basic image sanity
+    const int ch = frame_.stream.rgbImage.channels();
+    if (ch != 1 && ch != 3 && ch != 4) {
+        last_status_ = Status::BAD_IMAGE;
+        return false;
+    }
+
+    // depth requirement
+    if (opt.depth_policy == DepthPolicy::REQUIRE) {
+        if (frame_.stream.depthImage.empty()) {
+            last_status_ = Status::NO_DEPTH;
+            return false;
+        }
+    }
+
+    last_status_ = Status::OK;
+    return true;
+}
+
+bool AprilTagPerception::fetchEncoded(size_t encoded_index) {
+    if (!camera_) {
+        last_status_ = Status::NO_CAMERA;
+        return false;
+    }
+    try {
+        size_t idx = encoded_index;
+        camera_->getEncodedFrame(frame_.stream, idx);
+        // 注意：这里 frame_.stream.rgbFrame / depthFrame / codec / bKey 等由相机实现填充
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+bool AprilTagPerception::ensureGray() {
+    if (frame_.has_gray && !frame_.gray.empty()) {
+        return true;
+    }
+
+    const cv::Mat& color = frame_.stream.rgbImage;
+    if (color.empty()) {
+        last_status_ = Status::NO_NEW_FRAME;
+        return false;
+    }
+
+    const int ch = color.channels();
+    if (ch == 1) {
+        frame_.gray = color;
+    } else if (ch == 3) {
+        cv::cvtColor(color, frame_.gray, cv::COLOR_BGR2GRAY);
+    } else if (ch == 4) {
+        cv::cvtColor(color, frame_.gray, cv::COLOR_BGRA2GRAY);
+    } else {
+        last_status_ = Status::BAD_IMAGE;
+        return false;
+    }
+
+    if (frame_.gray.empty() || frame_.gray.type() != CV_8UC1) {
+        last_status_ = Status::BAD_IMAGE;
+        return false;
+    }
+
+    if (!frame_.gray.isContinuous()) {
+        frame_.gray = frame_.gray.clone();
+    }
+
+    frame_.has_gray = true;
+    return true;
+}
+
+bool AprilTagPerception::ensureVispImage() {
+    if (!ensureGray()) return false;
+
+    const int w = frame_.gray.cols;
+    const int h = frame_.gray.rows;
+
+    if (!frame_.has_visp || frame_.visp_w != w || frame_.visp_h != h) {
+        frame_.visp_I.resize(h, w);
+        frame_.visp_w = w;
+        frame_.visp_h = h;
+        frame_.has_visp = true;
+    }
+
+    for (int y = 0; y < h; ++y) {
+        std::memcpy(frame_.visp_I[y],
+                    frame_.gray.ptr<unsigned char>(y),
+                    static_cast<size_t>(w));
+    }
+    return true;
+}
+
+bool AprilTagPerception::detectTags() {
+    std::vector<vpHomogeneousMatrix> cMo_vec;
+    const bool detected = detector_.detect(frame_.visp_I, tag_size_m_, visp_cam_, cMo_vec);
+    if (!detected || cMo_vec.empty()) {
+        last_status_ = Status::NO_TAG;
+        return false;
+    }
+
+    const std::vector<int> tag_ids = detector_.getTagsId();
+    const std::vector<float> margins = detector_.getTagsDecisionMargin();
+    const size_t n = std::min(tag_ids.size(), cMo_vec.size());
+    if (n == 0) {
+        last_status_ = Status::NO_TAG;
+        return false;
+    }
+
+    tags_.reserve(n);
+    for (size_t i = 0; i < n; ++i) {
+        Tag t;
+        t.id = tag_ids[i];
+        t.cMo = cMo_vec[i];
+        t.T_c_t = toEigen4(cMo_vec[i]);
+
+        double w = 1.0;
+        if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
+            w = static_cast<double>(margins[i]);
+        }
+        t.margin = w;
+
+        id_to_index_[t.id] = tags_.size();
+        tags_.push_back(t);
+    }
+
+    last_status_ = Status::OK;
+    return true;
+}
+
+const AprilTagPerception::Tag* AprilTagPerception::findTag(int id) const {
+    const auto it = id_to_index_.find(id);
+    if (it == id_to_index_.end()) return nullptr;
+    return &tags_[it->second];
+}
+
+Eigen::Matrix4d AprilTagPerception::toEigen4(const vpHomogeneousMatrix& cMo) {
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    for (int r = 0; r < 3; ++r) {
+        for (int c = 0; c < 3; ++c) {
+            T(r, c) = cMo[r][c];
+        }
+    }
+    T(0, 3) = cMo[0][3];
+    T(1, 3) = cMo[1][3];
+    T(2, 3) = cMo[2][3];
+    return T;
+}
+
+} // namespace cmvr::perception

cmvr-es/perception/src/tag_relative_target_3d.cpp

@@ -2,26 +2,14 @@
 
 #include <algorithm>
 #include <cmath>
-#include <cstdint>
 #include <cstring>
 #include <limits>
 
-#include <opencv2/imgproc.hpp>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpImage.h>
-
 #include "common/utils/image/image_process.h"
 
 namespace cmvr::perception {
 namespace {
 
-// -------- helpers --------
-double sanitizeWeight(double w) {
-    if (!std::isfinite(w) || w <= 0.0) return 1.0;
-    return w;
-}
-
 class WeightedPointFusion {
 public:
     void reserve(size_t n) { points_.reserve(n); }
@@ -39,10 +27,8 @@ public:
 
     bool finalize(Eigen::Vector3d& p_c_out, int& best_id_out, double& spread_out) const {
         if (points_.empty() || weight_sum_ <= 0.0) return false;
-
         p_c_out = weighted_sum_ / weight_sum_;
         best_id_out = best_id_;
-
         double max_err = 0.0;
         for (const auto& p : points_) {
             max_err = std::max(max_err, (p - p_c_out).norm());
@@ -61,16 +47,10 @@ private:
 
 } // namespace
 
-TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
+TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<AprilTagPerception>& perception)
-    : camera_(camera) {
+    : perception_(perception) {}
-    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
-}
 
-void TagRelativeTarget3D::setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) {
+void TagRelativeTarget3D::clear() {
-    camera_ = camera;
-    frame_ = FrameCache{};
-    observations_.clear();
-    obs_frame_id_ = 0;
     anchors_in_tag_.clear();
     resetActiveTagTracking();
     resetLastOutputs();
@@ -81,10 +61,11 @@ const char* TagRelativeTarget3D::statusToString(Status status) {
     switch (status) {
         case Status::OK: return "ok";
         case Status::INVALID_INPUT: return "invalid_input";
-        case Status::NO_CAMERA: return "no_camera";
+        case Status::NO_PERCEPTION: return "no_perception";
-        case Status::NO_NEW_FRAME: return "no_new_frame";
+        case Status::PERCEPTION_NOT_READY: return "perception_not_ready";
-        case Status::BAD_IMAGE: return "bad_image";
         case Status::NO_TAG: return "no_tag";
+        case Status::NO_DEPTH: return "no_depth";
+        case Status::BAD_IMAGE: return "bad_image";
         case Status::NO_OBSERVATION: return "no_observation";
         case Status::TARGET_NOT_LOCKED: return "target_not_locked";
         case Status::NO_MATCHING_TAG: return "no_matching_tag";
@@ -92,29 +73,6 @@ const char* TagRelativeTarget3D::statusToString(Status status) {
     }
 }
 
-void TagRelativeTarget3D::clear() {
-    anchors_in_tag_.clear();
-    resetActiveTagTracking();
-    resetLastOutputs();
-
-    frame_ = FrameCache{};
-    observations_.clear();
-    obs_frame_id_ = 0;
-
-    last_status_ = Status::TARGET_NOT_LOCKED;
-}
-
-void TagRelativeTarget3D::resetLastOutputs() {
-    last_p_c_target_.setZero();
-    last_used_tag_id_ = -1;
-    last_spread_m_ = 0.0;
-    last_switched_ = false;
-}
-
-void TagRelativeTarget3D::setTagSize(double tag_size_m) {
-    if (std::isfinite(tag_size_m) && tag_size_m > 0.0) tag_size_m_ = tag_size_m;
-}
-
 void TagRelativeTarget3D::setDepthSamplingConfig(int window_size_px,
                                                  bool enable_outlier_reject,
                                                  double outlier_sigma,
@@ -125,7 +83,6 @@ void TagRelativeTarget3D::setDepthSamplingConfig(int window_size_px,
     depth_window_size_px_ = w;
 
     depth_outlier_reject_enabled_ = enable_outlier_reject;
-
     if (std::isfinite(outlier_sigma) && outlier_sigma > 0.0) depth_outlier_sigma_ = outlier_sigma;
     if (std::isfinite(outlier_min_dev_m) && outlier_min_dev_m >= 0.0) depth_outlier_min_dev_m_ = outlier_min_dev_m;
 }
@@ -151,190 +108,84 @@ void TagRelativeTarget3D::resetActiveTagTracking() {
     active_tag_missing_count_ = 0;
 }
 
-// =========================
+void TagRelativeTarget3D::resetLastOutputs() {
-// Internal frame grabbing
+    last_p_c_target_.setZero();
-// =========================
+    last_used_tag_id_ = -1;
-bool TagRelativeTarget3D::grabRGB() {
+    last_spread_m_ = 0.0;
-    if (!camera_) {
+    last_switched_ = false;
-        last_status_ = Status::NO_CAMERA;
+}
-        return false;
-    }
 
-    frame_.has_color = frame_.has_depth = frame_.has_intr = false;
+bool TagRelativeTarget3D::hasAnchorForTag(int tag_id) const {
+    return anchors_in_tag_.find(tag_id) != anchors_in_tag_.end();
+}
 
-    try {
+bool TagRelativeTarget3D::getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const {
-        camera_->getRGBImage(frame_.color, frame_.intr);
+    const auto it = anchors_in_tag_.find(tag_id);
-    } catch (...) {
+    if (it == anchors_in_tag_.end()) return false;
-        last_status_ = Status::NO_NEW_FRAME;
+    p_t_target_out = it->second;
-        return false;
-    }
-
-    if (frame_.color.empty()) {
-        last_status_ = Status::NO_NEW_FRAME;
-        return false;
-    }
-
-    frame_.has_color = true;
-    frame_.has_intr = true;
-    frame_.frame_id++;
     return true;
 }
 
-bool TagRelativeTarget3D::grabRGBD() {
+bool TagRelativeTarget3D::syncFromPerception(bool need_tags, bool need_depth) {
-    if (!camera_) {
+    if (!perception_) {
-        last_status_ = Status::NO_CAMERA;
+        last_status_ = Status::NO_PERCEPTION;
         return false;
     }
 
-    frame_.has_color = frame_.has_depth = frame_.has_intr = false;
+    // perception.update() 失败时，statusToString(perception->lastStatus()) 可用于上层统一打印
-
+    // 这里我们只根据缓存是否存在来决定是否能继续。
-    try {
+    const auto& color = perception_->color();
-        camera_->getRGBDImages(frame_.color, frame_.depth, frame_.intr);
+    if (color.empty()) {
-    } catch (...) {
+        last_status_ = Status::PERCEPTION_NOT_READY;
-        last_status_ = Status::NO_NEW_FRAME;
         return false;
     }
 
-    if (frame_.color.empty() || frame_.depth.empty()) {
+    intr_ = perception_->intrinsics();
-        last_status_ = Status::NO_NEW_FRAME;
+    depth_ = &perception_->depth();
-        return false;
+    used_frame_id_ = perception_->frameId();
-    }
 
-    frame_.has_color = true;
-    frame_.has_depth = true;
-    frame_.has_intr = true;
-    frame_.frame_id++;
-    return true;
-}
-
-bool TagRelativeTarget3D::updateInternal(bool need_depth, bool need_tags) {
-    // Always grab a NEW frame when called
     observations_.clear();
 
     if (need_depth) {
-        if (!grabRGBD()) return false;
+        if (!perception_->hasDepth() || depth_->empty()) {
-    } else {
+            last_status_ = Status::NO_DEPTH;
-        if (!grabRGB()) return false;
+            return false;
+        }
     }
 
     if (need_tags) {
-        if (!updateObservations()) return false;
+        if (!perception_->hasTags()) {
-    } else {
+            last_status_ = Status::NO_TAG;
-        last_status_ = Status::OK;
+            return false;
+        }
+        const auto& tags = perception_->tags();
+        observations_.reserve(tags.size());
+        for (const auto& t : tags) {
+            Observation obs;
+            obs.tag_id = t.id;
+            obs.T_c_t = t.T_c_t;
+            obs.weight = t.margin;
+            observations_.push_back(obs);
+        }
+        if (observations_.empty()) {
+            last_status_ = Status::NO_TAG;
+            return false;
+        }
     }
+
     return true;
 }
 
-bool TagRelativeTarget3D::updateObservations() {
-    if (!frame_.has_color || !frame_.has_intr) {
-        last_status_ = Status::BAD_IMAGE;
-        return false;
-    }
-
-    const cv::Mat& color = frame_.color;
-    if (color.empty()) {
-        last_status_ = Status::NO_NEW_FRAME;
-        return false;
-    }
-    if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
-        last_status_ = Status::BAD_IMAGE;
-        return false;
-    }
-
-    const double fx = static_cast<double>(frame_.intr.fx);
-    const double fy = static_cast<double>(frame_.intr.fy);
-    const double cx = static_cast<double>(frame_.intr.cx);
-    const double cy = static_cast<double>(frame_.intr.cy);
-    if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
-        last_status_ = Status::BAD_IMAGE;
-        return false;
-    }
-
-    cv::Mat gray;
-    if (color.channels() == 1) gray = color;
-    else if (color.channels() == 3) cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
-    else cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
-
-    if (gray.empty() || gray.type() != CV_8UC1) {
-        last_status_ = Status::BAD_IMAGE;
-        return false;
-    }
-    if (!gray.isContinuous()) gray = gray.clone();
-
-    const int width = gray.cols;
-    const int height = gray.rows;
-
-    vpCameraParameters cam;
-    cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
-
-    vpImage<unsigned char> I(height, width);
-    for (int y = 0; y < height; ++y) {
-        std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
-    }
-
-    std::vector<vpHomogeneousMatrix> cMo_vec;
-    const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
-    if (!detected || cMo_vec.empty()) {
-        last_status_ = Status::NO_TAG;
-        obs_frame_id_ = frame_.frame_id;
-        return false;
-    }
-
-    const std::vector<int> tag_ids = detector_.getTagsId();
-    const std::vector<float> margins = detector_.getTagsDecisionMargin();
-    const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
-    if (pair_size == 0) {
-        last_status_ = Status::NO_TAG;
-        obs_frame_id_ = frame_.frame_id;
-        return false;
-    }
-
-    observations_.clear();
-    observations_.reserve(pair_size);
-
-    for (size_t i = 0; i < pair_size; ++i) {
-        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
-        const vpHomogeneousMatrix& cMo = cMo_vec[i];
-
-        for (int r = 0; r < 3; ++r) {
-            for (int c = 0; c < 3; ++c) T(r, c) = cMo[r][c];
-        }
-        T(0, 3) = cMo[0][3];
-        T(1, 3) = cMo[1][3];
-        T(2, 3) = cMo[2][3];
-
-        double w = 1.0;
-        if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
-            w = static_cast<double>(margins[i]);
-        }
-
-        observations_.push_back(TagPoseObservation{tag_ids[i], T, w});
-    }
-
-    if (observations_.empty()) {
-        last_status_ = Status::NO_TAG;
-        obs_frame_id_ = frame_.frame_id;
-        return false;
-    }
-
-    obs_frame_id_ = frame_.frame_id;
-    last_status_ = Status::OK;
-    return true;
-}
-
-// =========================
-// Public APIs (u,v only)
-// =========================
 bool TagRelativeTarget3D::solveFromPixel(int u, int v) {
     resetLastOutputs();
 
     if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) {
-        if (!updateInternal(true, false)) return false;           // RGBD only
+        if (!syncFromPerception(false, true)) return false;
-        return solveFromPixelWithDepthCached(u, v);
+        return solveFromPixelWithDepth(u, v);
     }
 
-    if (!updateInternal(false, true)) return false;               // RGB + tags
+    if (!syncFromPerception(true, false)) return false;
-    return solveFromPixelOnPlaneCached(u, v);
+    return solveFromPixelOnPlane(u, v);
 }
 
 bool TagRelativeTarget3D::startTrackingFromPixel(int u, int v) {
@@ -342,21 +193,19 @@ bool TagRelativeTarget3D::startTrackingFromPixel(int u, int v) {
     resetActiveTagTracking();
 
     const bool need_depth = (target_point_method_ == TargetPointMethod::DEPTH_IMAGE);
-    if (!updateInternal(need_depth, true)) return false;          // 追踪必须要 tags
+    // 追踪必须要 tags 建锚点；深度法仅用于 lock 点
+    if (!syncFromPerception(true, need_depth)) return false;
 
-    // 先求 lock 点（写入 last_*）
+    // 1) 先算 lock 点（写 last_*）
     if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) {
-        if (!solveFromPixelWithDepthCached(u, v)) return false;
+        if (!solveFromPixelWithDepth(u, v)) return false;
-        // 深度法：used_tag=-1, spread=0（已经在 solveFromPixelWithDepthCached 里写好）
+        // 深度法：last_used_tag_id_=-1，last_spread_m_=0
     } else {
-        if (!solveFromPixelOnPlaneCached(u, v)) return false;
+        if (!solveFromPixelOnPlane(u, v)) return false;
     }
 
-    // 用 lock 点建锚点 + 选 active
+    // 2) 建锚点 + 选 active
-    const Eigen::Vector3d p_lock = last_p_c_target_;
+    return initTrackingFromResolvedPoint(last_p_c_target_, last_used_tag_id_, last_spread_m_);
-    const int used_tag = last_used_tag_id_;
-    const double spread = last_spread_m_;
-    return initTrackingFromResolvedPoint(p_lock, used_tag, spread);
 }
 
 bool TagRelativeTarget3D::track() {
@@ -366,92 +215,20 @@ bool TagRelativeTarget3D::track() {
         last_status_ = Status::TARGET_NOT_LOCKED;
         return false;
     }
+    // 跟踪只需要 tags（不需要 depth）
+    if (!syncFromPerception(true, false)) return false;
 
-    if (!updateInternal(false, true)) return false; // RGB + tags
+    return trackWithActiveTag();
-    return trackWithActiveTagFromCached();
 }
 
-// =========================
+bool TagRelativeTarget3D::solveFromPixelOnPlane(int u, int v) {
-// Internal anchor ops
-// =========================
-bool TagRelativeTarget3D::lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing) {
-    if (!isFiniteVector(p_c_target)) {
-        last_status_ = Status::INVALID_INPUT;
-        return false;
-    }
-    if (observations_.empty()) {
-        last_status_ = Status::NO_OBSERVATION;
-        return false;
-    }
-
-    size_t updated = 0;
-    for (const auto& obs : observations_) {
-        if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
-        if (!overwrite_existing && anchors_in_tag_.find(obs.tag_id) != anchors_in_tag_.end()) continue;
-
-        anchors_in_tag_[obs.tag_id] = pointCameraToTag(obs.T_c_t, p_c_target);
-        ++updated;
-    }
-
-    if (updated == 0) {
-        last_status_ = Status::INVALID_INPUT;
-        return false;
-    }
-
-    last_status_ = Status::OK;
-    return true;
-}
-
-bool TagRelativeTarget3D::resolveTargetInCameraInternal() {
-    if (anchors_in_tag_.empty()) {
-        last_status_ = Status::TARGET_NOT_LOCKED;
-        return false;
-    }
-    if (observations_.empty()) {
-        last_status_ = Status::NO_OBSERVATION;
-        return false;
-    }
-
-    WeightedPointFusion fusion;
-    fusion.reserve(observations_.size());
-
-    for (const auto& obs : observations_) {
-        if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
-        const auto it = anchors_in_tag_.find(obs.tag_id);
-        if (it == anchors_in_tag_.end()) continue;
-
-        const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second);
-        if (!isFiniteVector(p_c)) continue;
-
-        fusion.addPoint(obs.tag_id, p_c, sanitizeObsWeight(obs.weight));
-    }
-
-    Eigen::Vector3d p_out = Eigen::Vector3d::Zero();
-    int used = -1;
-    double spread = 0.0;
-    if (!fusion.finalize(p_out, used, spread)) {
-        last_status_ = Status::NO_MATCHING_TAG;
-        return false;
-    }
-
-    last_p_c_target_ = p_out;
-    last_used_tag_id_ = used;
-    last_spread_m_ = spread;
-    last_status_ = Status::OK;
-    return true;
-}
-
-// =========================
-// Internal solvers (cached)
-// =========================
-bool TagRelativeTarget3D::solveFromPixelOnPlaneCached(int u, int v) {
     if (observations_.empty()) {
         last_status_ = Status::NO_OBSERVATION;
         return false;
     }
 
     Eigen::Vector3d ray_c = Eigen::Vector3d::Zero();
-    if (!cmvr::ImageProcess::pixelToRayCamera(frame_.intr, u, v, ray_c)) {
+    if (!cmvr::ImageProcess::pixelToRayCamera(intr_, u, v, ray_c)) {
         last_status_ = Status::INVALID_INPUT;
         return false;
     }
@@ -471,12 +248,13 @@ bool TagRelativeTarget3D::solveFromPixelOnPlaneCached(int u, int v) {
         if (!intersectRayWithTagPlane(obs.T_c_t, ray_c, p_c_intersection, &view_cos)) continue;
         if (view_cos < kMinViewCos) continue;
 
-        const double w_obs = sanitizeObsWeight(obs.weight);
+        const double w_obs = sanitizeWeight(obs.weight);
 
+        // 像素邻近性：tag 中心投影距离越近越可信
         const Eigen::Vector3d tag_center_c = obs.T_c_t.block<3, 1>(0, 3);
         Eigen::Vector2d uv_tag = Eigen::Vector2d::Zero();
         double dist_px = kDistNormPx;
-        if (cmvr::ImageProcess::projectCameraPointToPixel(frame_.intr, tag_center_c, uv_tag)) {
+        if (cmvr::ImageProcess::projectCameraPointToPixel(intr_, tag_center_c, uv_tag)) {
             dist_px = (uv_tag - uv_target).norm();
         }
         const double dist_gain = 1.0 / (1.0 + dist_px / kDistNormPx);
@@ -502,15 +280,15 @@ bool TagRelativeTarget3D::solveFromPixelOnPlaneCached(int u, int v) {
     return true;
 }
 
-bool TagRelativeTarget3D::solveFromPixelWithDepthCached(int u, int v) {
+bool TagRelativeTarget3D::solveFromPixelWithDepth(int u, int v) {
-    if (!frame_.has_depth || frame_.depth.empty() || !frame_.has_intr) {
+    if (!depth_ || depth_->empty()) {
-        last_status_ = Status::BAD_IMAGE;
+        last_status_ = Status::NO_DEPTH;
         return false;
     }
 
     Eigen::Vector3d p = Eigen::Vector3d::Zero();
-    if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(frame_.intr,
+    if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(intr_,
-                                                         frame_.depth,
+                                                         *depth_,
                                                          u,
                                                          v,
                                                          p,
@@ -529,10 +307,8 @@ bool TagRelativeTarget3D::solveFromPixelWithDepthCached(int u, int v) {
     return true;
 }
 
-bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock,
+bool TagRelativeTarget3D::lockAnchorsFromPoint(const Eigen::Vector3d& p_c_target, bool overwrite_existing) {
-                                                       int used_tag,
+    if (!isFiniteVector(p_c_target)) {
-                                                       double spread) {
-    if (!isFiniteVector(p_c_lock)) {
         last_status_ = Status::INVALID_INPUT;
         return false;
     }
@@ -541,10 +317,30 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p
         return false;
     }
 
-    // 建锚点（覆盖）
+    size_t updated = 0;
-    if (!lockTargetInCameraInternal(p_c_lock, true)) return false;
+    for (const auto& obs : observations_) {
+        if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
+
+        if (!overwrite_existing && anchors_in_tag_.find(obs.tag_id) != anchors_in_tag_.end()) continue;
+
+        anchors_in_tag_[obs.tag_id] = pointCameraToTag(obs.T_c_t, p_c_target);
+        ++updated;
+    }
+
+    if (updated == 0) {
+        last_status_ = Status::INVALID_INPUT;
+        return false;
+    }
+
+    last_status_ = Status::OK;
+    return true;
+}
+
+bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock,
+                                                         int used_tag,
+                                                         double spread) {
+    if (!lockAnchorsFromPoint(p_c_lock, true)) return false;
 
-    // 选 active tag
     int active_tag = used_tag;
     if (active_tag < 0 || !hasAnchorForTag(active_tag)) {
         double best_score = -std::numeric_limits<double>::infinity();
@@ -565,7 +361,6 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p
     active_tag_id_ = active_tag;
     active_tag_missing_count_ = 0;
 
-    // 输出保持 lock 点
     last_p_c_target_ = p_c_lock;
     last_used_tag_id_ = used_tag;
     last_spread_m_ = spread;
@@ -574,12 +369,7 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p
     return true;
 }
 
-bool TagRelativeTarget3D::trackWithActiveTagFromCached() {
+bool TagRelativeTarget3D::trackWithActiveTag() {
-    if (observations_.empty()) {
-        last_status_ = Status::NO_OBSERVATION;
-        return false;
-    }
-
     int active_obs_index = -1;
     double active_score = -std::numeric_limits<double>::infinity();
 
@@ -647,44 +437,25 @@ bool TagRelativeTarget3D::trackWithActiveTagFromCached() {
         return false;
     }
 
+
+    // 动态补 p_t_target
+    lockAnchorsFromPoint(p_c, false);
+
     last_p_c_target_ = p_c;
     last_used_tag_id_ = obs.tag_id;
-    last_spread_m_ = 0.0;      // 单 tag
+    last_spread_m_ = 0.0;
     last_switched_ = switched;
     last_status_ = Status::OK;
     return true;
 }
 
-// =========================
+double TagRelativeTarget3D::trackingCandidateScore(const Observation& obs) const {
-// Diagnostics
-// =========================
-bool TagRelativeTarget3D::hasAnchorForTag(int tag_id) const {
-    return anchors_in_tag_.find(tag_id) != anchors_in_tag_.end();
-}
-
-bool TagRelativeTarget3D::getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const {
-    const auto it = anchors_in_tag_.find(tag_id);
-    if (it == anchors_in_tag_.end()) return false;
-    p_t_target_out = it->second;
-    return true;
-}
-
-// =========================
-// math helpers
-// =========================
-double TagRelativeTarget3D::sanitizeObsWeight(double w) {
-    return sanitizeWeight(w);
-}
-
-double TagRelativeTarget3D::trackingCandidateScore(const TagPoseObservation& obs) const {
     if (obs.tag_id < 0) return -std::numeric_limits<double>::infinity();
-
     const auto it = anchors_in_tag_.find(obs.tag_id);
     if (it == anchors_in_tag_.end()) return -std::numeric_limits<double>::infinity();
 
-    const double w = std::max(1e-12, sanitizeObsWeight(obs.weight));
+    const double w = std::max(1e-12, sanitizeWeight(obs.weight));
     const Eigen::Vector3d& p_t_target = it->second;
-
     const double dist_in_tag = std::hypot(p_t_target.x(), p_t_target.y());
     const double proximity_gain = 1.0 / (1.0 + dist_in_tag / candidate_proximity_scale_m_);
 
@@ -696,6 +467,12 @@ double TagRelativeTarget3D::trackingCandidateScore(const TagPoseObservation& obs
     return score;
 }
 
+// -------- math helpers --------
+double TagRelativeTarget3D::sanitizeWeight(double w) {
+    if (!std::isfinite(w) || w <= 0.0) return 1.0;
+    return w;
+}
+
 bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) {
     for (int r = 0; r < 4; ++r)
         for (int c = 0; c < 4; ++c)
@@ -708,29 +485,29 @@ bool TagRelativeTarget3D::isFiniteVector(const Eigen::Vector3d& p) {
 }
 
 Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t,
-                                                      const Eigen::Vector3d& p_t) {
+                                                       const Eigen::Vector3d& p_t) {
     const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
     const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
     return R * p_t + t;
 }
 
 Eigen::Vector3d TagRelativeTarget3D::pointCameraToTag(const Eigen::Matrix4d& T_c_t,
-                                                      const Eigen::Vector3d& p_c) {
+                                                       const Eigen::Vector3d& p_c) {
     const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
     const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
     return R.transpose() * (p_c - t);
 }
 
 bool TagRelativeTarget3D::intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
-                                                   const Eigen::Vector3d& ray_c,
+                                                    const Eigen::Vector3d& ray_c,
-                                                   Eigen::Vector3d& p_c_intersection,
+                                                    Eigen::Vector3d& p_c_intersection,
-                                                   double* view_cos_out) {
+                                                    double* view_cos_out) {
     if (!isFiniteMatrix(T_c_t) || !isFiniteVector(ray_c)) return false;
 
     const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
     const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
 
-    Eigen::Vector3d n = R.col(2); // tag 局部 z 轴
+    Eigen::Vector3d n = R.col(2); // tag 平面法向（tag局部z轴）
     const double n_norm = n.norm();
     if (!std::isfinite(n_norm) || n_norm <= 1e-12) return false;
     n /= n_norm;
@@ -740,10 +517,10 @@ bool TagRelativeTarget3D::intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
     const Eigen::Vector3d d = ray_c / ray_norm;
 
     const double denom = n.dot(d);
-    if (!std::isfinite(denom) || std::abs(denom) <= 1e-8) return false;
+    if (!std::isfinite(denom) || std::abs(denom) <= 1e-8) return false; // nearly parallel
 
     const double lambda = n.dot(t) / denom;
-    if (!std::isfinite(lambda) || lambda <= 1e-8) return false;
+    if (!std::isfinite(lambda) || lambda <= 1e-8) return false; // behind camera
 
     p_c_intersection = lambda * d;
     if (!isFiniteVector(p_c_intersection)) return false;

cmvr-es/perception/src/tag_relative_target_3d_test.cpp

@@ -297,15 +297,21 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
     cam_cfg.set_fps(fps);
     cam_cfg.set_codec("H265");
     cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
-    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGB);
+    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
-    // cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
+    cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
     cam_cfg.set_buffer_size(30);
     cam_cfg.set_sync(true);
     cam_cfg.set_enable(true);
 
     auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
-    cmvr::perception::TagRelativeTarget3D tracker(camera);
+
-    tracker.setTagSize(tag_size);
+    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
+    perception->setTagSize(tag_size);
+    cmvr::perception::TagRelativeTarget3D tracker(perception);
+
+    cmvr::perception::AprilTagPerception::Options opt;
+    opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
+    opt.detect_tags = true;
     tracker.setActiveTagSwitchPolicy(4, 1.2);
     tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
     tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
@@ -413,6 +419,7 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
         std::string phase = target_locked ? "track" : "lock";
 
         if (!target_locked) {
+            perception->update(opt);
             if (tracker.startTrackingFromPixel(target_u, target_v)) {
                 p_c_target = tracker.lastTargetInCamera();
                 used_tag_id = tracker.lastUsedTagId();
@@ -439,11 +446,13 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
             }
         } else {
             const int old_active_tag = tracker.activeTagId();
+            perception->update(opt);
             if (tracker.track()) {
                 p_c_target = tracker.lastTargetInCamera();
                 used_tag_id = tracker.lastUsedTagId();
                 spread_m = tracker.lastSpread();
                 const bool switched = tracker.lastSwitched();
+
                 target_valid = true;
                 ok = true;
                 ++track_success_count;
@@ -458,7 +467,8 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
                           << " used_tag=" << used_tag_id
                           << " active_tag=" << tracker.activeTagId()
                           << " spread=" << spread_m
-                          << "\n";
+                        << "anchorCount=" << tracker.anchorCount()
+                          << std::endl;
             } else {
                 ++track_fail_count;
                 if (((i + 1) % kLogEveryNFrames) == 0) {
@@ -595,109 +605,109 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
               << "\n";
 }
 
-TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) {
+// TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) {
-    const std::string serial = kRsSerial;
+//     const std::string serial = kRsSerial;
-    if (serial.empty()) {
+//     if (serial.empty()) {
-        GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
+//         GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
-    }
+//     }
-
+//
-    cmvr::config::RealSenseCameraConfig cam_cfg;
+//     cmvr::config::RealSenseCameraConfig cam_cfg;
-    cam_cfg.set_id("tag_relative_target_3d_test_no_display");
+//     cam_cfg.set_id("tag_relative_target_3d_test_no_display");
-    cam_cfg.set_serialnumber(serial);
+//     cam_cfg.set_serialnumber(serial);
-    cam_cfg.set_width(kWidth);
+//     cam_cfg.set_width(kWidth);
-    cam_cfg.set_height(kHeight);
+//     cam_cfg.set_height(kHeight);
-    cam_cfg.set_fps(kFps);
+//     cam_cfg.set_fps(kFps);
-    cam_cfg.set_codec("H265");
+//     cam_cfg.set_codec("H265");
-    cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
+//     cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
-    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
+//     cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
-    cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
+//     cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
-    cam_cfg.set_buffer_size(30);
+//     cam_cfg.set_buffer_size(30);
-    cam_cfg.set_sync(true);
+//     cam_cfg.set_sync(true);
-    cam_cfg.set_enable(true);
+//     cam_cfg.set_enable(true);
-
+//
-    auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
+//     auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
-    cmvr::perception::TagRelativeTarget3D tracker(camera);
+//     cmvr::perception::TagRelativeTarget3D tracker(camera);
-    ASSERT_NO_THROW(camera->init());
+//     ASSERT_NO_THROW(camera->init());
-    ASSERT_NO_THROW(camera->start());
+//     ASSERT_NO_THROW(camera->start());
-    struct CameraStopGuard {
+//     struct CameraStopGuard {
-        std::shared_ptr<cmvr::device::RealsenseCamera> cam;
+//         std::shared_ptr<cmvr::device::RealsenseCamera> cam;
-        ~CameraStopGuard() {
+//         ~CameraStopGuard() {
-            if (!cam) return;
+//             if (!cam) return;
-            try {
+//             try {
-                cam->stop();
+//                 cam->stop();
-            } catch (...) {
+//             } catch (...) {
-            }
+//             }
-        }
+//         }
-    } stop_guard{camera};
+//     } stop_guard{camera};
-    tracker.setTagSize(kTagSize);
+//     tracker.setTagSize(kTagSize);
-    tracker.setActiveTagSwitchPolicy(4, 1.2);
+//     tracker.setActiveTagSwitchPolicy(4, 1.2);
-    tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08);
+//     tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08);
-    // 深度法采样：5x5 中值 + MAD 离群剔除。
+//     // 深度法采样：5x5 中值 + MAD 离群剔除。
-    tracker.setDepthSamplingConfig(5, true, 2.5, 0.003);
+//     tracker.setDepthSamplingConfig(5, true, 2.5, 0.003);
-    const int tries = kTries;
+//     const int tries = kTries;
-    const bool infinite = (tries <= 0);
+//     const bool infinite = (tries <= 0);
-    int plane_fail_count = 0;
+//     int plane_fail_count = 0;
-    int depth_fail_count = 0;
+//     int depth_fail_count = 0;
-    int plane_ok_count = 0;
+//     int plane_ok_count = 0;
-    int depth_ok_count = 0;
+//     int depth_ok_count = 0;
-    int both_ok_count = 0;
+//     int both_ok_count = 0;
-    int printed_count = 0;
+//     int printed_count = 0;
-    bool got_target = false;
+//     bool got_target = false;
-    const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2);
+//     const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2);
-    const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2);
+//     const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2);
-
+//
-    for (int i = 0; infinite || i < tries; ++i) {
+//     for (int i = 0; infinite || i < tries; ++i) {
-        // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。
+//         // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。
-        tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
+//         tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
-        const bool plane_ok = tracker.solveFromPixel(target_u, target_v);
+//         const bool plane_ok = tracker.solveFromPixel(target_u, target_v);
-        const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
+//         const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
-        const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1;
+//         const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1;
-        const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0;
+//         const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0;
-        if (plane_ok) {
+//         if (plane_ok) {
-            ++plane_ok_count;
+//             ++plane_ok_count;
-        } else {
+//         } else {
-            ++plane_fail_count;
+//             ++plane_fail_count;
-        }
+//         }
-
+//
-        tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE);
+//         tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE);
-        const bool depth_ok = tracker.solveFromPixel(target_u, target_v);
+//         const bool depth_ok = tracker.solveFromPixel(target_u, target_v);
-        const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
+//         const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
-        if (depth_ok) {
+//         if (depth_ok) {
-            ++depth_ok_count;
+//             ++depth_ok_count;
-        } else {
+//         } else {
-            ++depth_fail_count;
+//             ++depth_fail_count;
-        }
+//         }
-
+//
-        if (plane_ok && depth_ok) {
+//         if (plane_ok && depth_ok) {
-            ++both_ok_count;
+//             ++both_ok_count;
-        }
+//         }
-        if (plane_ok || depth_ok) {
+//         if (plane_ok || depth_ok) {
-            got_target = true;
+//             got_target = true;
-        }
+//         }
-
+//
-        std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] "
+//         std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] "
-                  << "uv=[" << target_u << "," << target_v << "] "
+//                   << "uv=[" << target_u << "," << target_v << "] "
-                  << "plane_ok=" << plane_ok
+//                   << "plane_ok=" << plane_ok
-                  << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]")
+//                   << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]")
-                  << " used_tag=" << plane_used_tag
+//                   << " used_tag=" << plane_used_tag
-                  << " spread=" << plane_spread
+//                   << " spread=" << plane_spread
-                  << " | depth_ok=" << depth_ok
+//                   << " | depth_ok=" << depth_ok
-                  << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]");
+//                   << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]");
-        if (plane_ok && depth_ok) {
+//         if (plane_ok && depth_ok) {
-            std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm();
+//             std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm();
-        }
+//         }
-        std::cout << std::endl;
+//         std::cout << std::endl;
-
+//
-        ++printed_count;
+//         ++printed_count;
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
+//         std::this_thread::sleep_for(std::chrono::milliseconds(30));
-    }
+//     }
-
+//
-    EXPECT_TRUE(got_target)
+//     EXPECT_TRUE(got_target)
-        << "Failed to solve target point in camera frame. "
+//         << "Failed to solve target point in camera frame. "
-        << " plane_fail=" << plane_fail_count
+//         << " plane_fail=" << plane_fail_count
-        << " depth_fail=" << depth_fail_count
+//         << " depth_fail=" << depth_fail_count
-        << " plane_ok=" << plane_ok_count
+//         << " plane_ok=" << plane_ok_count
-        << " depth_ok=" << depth_ok_count
+//         << " depth_ok=" << depth_ok_count
-        << " both_ok=" << both_ok_count
+//         << " both_ok=" << both_ok_count
-        << " printed=" << printed_count;
+//         << " printed=" << printed_count;
-}
+// }

model/xiaoyan_description/dual_arm.xml

@@ -66,7 +66,7 @@
   </asset>
 
   <worldbody>
-    <body name="tag_board" pos="0.7 -0.2 1.05" euler="1.37 -1.57 0">
+    <body name="tag_board" pos="0.7 -0.2 1.05" euler="1.57 -1.57 0">
 
       <!-- 15cm x 15cm 白板，同时贴 apriltag 纹理（纹理里自带白边） -->
       <geom name="tag_board_geom"