diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 13bc765b..19bdfe64 100644 --- a/cmvr-es/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 1f4f1267..38902ace 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/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) \ No newline at end of file +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 +) diff --git a/cmvr-es/common/utils/visualization/image_display.h b/cmvr-es/common/utils/visualization/image_display.h new file mode 100644 index 00000000..67b3f781 --- /dev/null +++ b/cmvr-es/common/utils/visualization/image_display.h @@ -0,0 +1,609 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +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(b), + static_cast(g), + static_cast(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 texts_; // 当前待绘制的文本叠加列表。 + std::vector points_; // 当前待绘制的点叠加列表。 + std::vector circles_; // 当前待绘制的圆圈叠加列表。 + std::vector crosses_; // 当前待绘制的十字叠加列表。 + bool window_created_{false}; // 显示窗口是否已经创建。 +}; + +} // namespace cmvr::common diff --git a/cmvr-es/common/utils/visualization/image_display_test.cpp b/cmvr-es/common/utils/visualization/image_display_test.cpp new file mode 100644 index 00000000..78319116 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +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(std::lround(uv.x())); + pixel_out.v = static_cast(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(40, 60); + EXPECT_GT(static_cast(center[2]), 150); + EXPECT_LT(static_cast(center[1]), 80); + EXPECT_LT(static_cast(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(cam_cfg); + auto perception = std::make_shared(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 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(); +} diff --git a/cmvr-es/controller/CMakeLists.txt b/cmvr-es/controller/CMakeLists.txt index 01685ec0..86b3027a 100644 --- a/cmvr-es/controller/CMakeLists.txt +++ b/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 diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index dcaa3eec..f32af6c7 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/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 #include @@ -13,52 +11,53 @@ #include #include -#include #include #include -#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 位姿估计得到的深度。 */ - PREFER_DEPTH, /**< 优先使用深度图；无效时回退到位姿深度。 */ - DEPTH_ONLY /**< 必须使用深度图；无效则本次计算失败。 */ + MONOCULAR = 0, + PREFER_DEPTH, + DEPTH_ONLY }; - /** - * @brief `compute()` 最近一次状态。 - */ enum class ComputeStatus { - OK = 0, /**< 计算成功。 */ - NOT_READY, /**< 控制器未初始化完成。 */ - NO_NEW_FRAME, /**< 未取到可用新帧。 */ - BAD_IMAGE, /**< 图像格式或数据异常。 */ - INVALID_INPUT, /**< 输入参数异常。 */ - NO_DEPTH, /**< 需要深度但深度不可用。 */ - NO_TAG, /**< 未检测到 AprilTag。 */ - TAG_MISMATCH, /**< 检测到 AprilTag，但与指定 id 不匹配。 */ - IK_FAILED /**< 速度 IK 求解失败。 */ + OK = 0, + NOT_READY, + NO_NEW_FRAME, + BAD_IMAGE, + INVALID_INPUT, + NO_DEPTH, + NO_TAG, + TAG_MISMATCH, + IK_FAILED }; - /** - * @brief 最近一次成功检测时使用的深度来源。 - */ enum class DepthUsage { - NONE = 0, /**< 本帧未使用深度。 */ - POSE_ONLY, /**< 使用位姿估计深度。 */ - DEPTH_ONLY, /**< 使用深度图深度。 */ - MIXED /**< 混合使用（预留）。 */ + NONE = 0, + POSE_ONLY, + DEPTH_ONLY, + MIXED }; /** @@ -67,68 +66,73 @@ public: IbvsController(); /** - * @brief 初始化控制器与 DLS 速度 IK 求解器。 - * @param camera 相机对象。 + * @brief 初始化 DLS 速度 IK 求解器（不再需要相机）。 * @param urdf_path URDF 文件路径。 - * @param base_link 基坐标系 link 名称。 - * @param flange_link 法兰 link 名称。 - * @param camera_link 相机末端 link 名称（用于速度 IK）。 + * @param base_link IK 链基座 link 名称。 + * @param flange_link IK 链末端法兰 link 名称。 + * @param camera_link URDF 中相机 link 名称，对应坐标系 `u`。 * @return 初始化成功返回 `true`。 */ - bool init(const std::shared_ptr& camera, - const std::string& urdf_path, + bool init(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& perception); + + const std::shared_ptr& perception() const { return perception_; } + /** * @brief 重置内部状态与关节命令缓存。 - * @param q_init 初始关节命令；为空时清空缓存。 + * @param q_init 初始关节位置命令；为空时清空内部缓存。 */ void reset(const std::vector& q_init = {}); /** - * @brief 根据当前图像与关节角计算下一拍关节位置命令。 + * @brief 根据当前关节状态计算下一拍关节位置命令。 * @param joints_angle 当前关节角。 - * @param dt 控制周期（秒）。 + * @param dt 控制周期，单位秒。 * @param q_cmd_out 输出的下一拍关节位置命令。 * @return 计算成功返回 `true`。 */ bool compute(const std::vector& joints_angle, double dt, std::vector& q_cmd_out); + /** - * @brief 根据当前图像与关节角计算目标关节速度命令。 + * @brief 根据当前关节状态计算关节速度命令。 * @param joints_angle 当前关节角。 - * @param qdot_out 输出的关节速度命令（rad/s）。 + * @param qdot_out 输出的关节速度命令。 * @return 计算成功返回 `true`。 */ bool compute(const std::vector& joints_angle, std::vector& qdot_out); /** - * @brief 设置 ViSP 控制增益。 - * @param lambda 控制增益 `lambda`。 + * @brief 设置 ViSP 视觉伺服增益。 + * @param lambda 视觉伺服增益。 */ void setLambda(double lambda); + /** - * @brief 设置 AprilTag 边长。 - * @param tag_size_m tag 边长（米）。 - */ - void setTagSize(double tag_size_m); - /** - * @brief 设置要跟踪的 tag id（必填）。 - * @param tag_id 指定 tag id，必须 >= 0。 + * @brief 设置当前需要跟踪的 tag id。 + * @param tag_id 目标 tag id。 */ void setTrackedTagId(int tag_id); + /** * @brief 设置期望目标位姿（`cMo_des tag 坐标系相对于VISP相机的期望位姿`）。 - * @param x 期望平移 x（米）。 - * @param y 期望平移 y（米）。 - * @param z 期望平移 z（米）。 - * @param rx 期望旋转参数 rx（弧度，直接传给 `vpRotationMatrix::buildFrom`）。 - * @param ry 期望旋转参数 ry（弧度，直接传给 `vpRotationMatrix::buildFrom`）。 - * @param rz 期望旋转参数 rz（弧度，直接传给 `vpRotationMatrix::buildFrom`）。 + * @param x 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。 + * @param y 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。 + * @param z 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。 + * @param rx 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度，直接传给 `vpRotationMatrix::buildFrom`。 + * @param ry 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度，直接传给 `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 设置关节速度绝对值上限。 - * @param qdot_max 关节最大速度（rad/s）。 + * @brief 设置每个关节速度命令的绝对值上限。 + * @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& vmax6); + /** - * @brief 设置关节限位避障 null-space 项参数。 - * + * @brief 设置关节限位回避参数。 * @param enable 是否启用。 - * @param gain 避障增益（rad/s 量纲）。 - * @param margin_ratio 上下限触发边界占关节行程比例 (0, 0.5)。 - * @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。 + * @param gain 回避增益。 + * @param margin_ratio 触发限位回避的边界比例。 + * @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); - /** - * @brief 最近一帧是否检测到“指定 id 的 tag”。 - * @return 检测到返回 `true`。 - */ + // 最近一帧是否检测到了当前被跟踪的 tag。 bool isTagDetected() const { return last_tag_detected_; } - /** - * @brief 获取最近一次计算状态。 - * @return 计算状态。 - */ + + // 最近一次 `compute()` 的状态。 ComputeStatus lastComputeStatus() const { return last_compute_status_; } - /** - * @brief 计算状态转字符串。 - * @param status 状态枚举。 - * @return 状态字符串。 - */ + + // 计算状态转字符串。 static const char* statusToString(ComputeStatus status); - /** - * @brief 获取最近一帧深度来源。 - * @return 深度来源枚举。 - */ + + // 最近一次 `compute()` 实际使用的深度来源。 DepthUsage lastDepthUsage() const { return last_depth_usage_; } - /** - * @brief 深度来源转字符串。 - * @param usage 深度来源枚举。 - * @return 深度来源字符串。 - */ + + // 深度来源转字符串。 static const char* depthUsageToString(DepthUsage usage); - /** - * @brief 获取最近检测到的 tag 平移（ViSP 相机坐标系）。 - * @return tag 平移向量。 - */ + + // 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。 const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; } - /** - * @brief 获取当前指定的 tag id。 - * @return 指定的 tag id；若未设置返回 `-1`。 - */ + + // 当前配置要跟踪的 tag id。 int trackedTagId() const { return tracked_tag_id_; } - /** - * @brief 获取最近一次用于控制的 tag id。 - * @return 最近使用的 tag id；若本帧未成功使用返回 `-1`。 - */ + + // 最近一次成功控制时实际使用的 tag id。 int lastUsedTagId() const { return last_used_tag_id_; } - /** - * @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。 - * @return 六维 twist 向量。 - */ + + // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。 const Eigen::Matrix& 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& joints_angle, std::vector& qdot_out); + /** - * @brief 按 URDF 关节上下限对关节命令做硬裁剪。 - * @param q 关节命令，函数内原地修改。 + * @brief 按 URDF 关节上下限对关节命令做原地裁剪。 + * @param q 关节命令向量。 */ void clampJointCommandInPlace(std::vector& 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 camera_{nullptr}; - // 参数 - // ViSP 控制增益。 + // IK 链基座 frame 名称。 + std::string base_frame_name_; + + // URDF 中相机 frame 名称，对应坐标系 `u`。 + std::string camera_frame_name_; + + // 共享感知前端。 + std::shared_ptr 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 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()}; - // 坐标对齐 - // R = R_camera_urdf_ * （R_cv_转置） == ViSP相机坐标系 到 URDF 相机系 的等效旋转 - // AbstractCamera -> ViSP 的旋转矩阵。 + // 从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。 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 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（必填）；<0 表示未设置。 + + // 当前配置要跟踪的 tag id。 int tracked_tag_id_{-1}; // 速度 IK 求解器。 std::unique_ptr 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 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 last_v_camera_visp_{Eigen::Matrix::Zero()}; }; } // namespace cmvr + +#endif // CMVR_IBVS_CONTROLLER_H diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/controller/src/controller_test.cpp index 247d9e93..579230e0 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/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_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25); + ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.0, 0), + 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(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 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 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 ibvs_controller_{nullptr}; std::shared_ptr mujoco_camera_{nullptr}; + std::shared_ptr perception_{nullptr}; + cmvr::perception::AprilTagPerception::Options perception_opt_{}; std::array q_cmd_{{0, 0, 0, 0, 0, 0, 0}}; int step_count_{0}; diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index 5f5adaee..91979e99 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/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 #include -#include -#include -#include +#include +#include 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() - : detector_(vpDetectorAprilTag::TAG_36h11) { - // AbstractCamera 相机系 -> ViSP 相机系 - R_cv_ = (Eigen::Matrix3d() << - 1, 0, 0, - 0, 1, 0, - 0, 0, 1).finished(); +IbvsController::IbvsController() { + R_cv_.setIdentity(); + R_camera_urdf_.setIdentity(); - // 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& camera, - const std::string& urdf_path, +bool IbvsController::init(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( 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& 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& 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& q_init) { if (q_init.empty()) { q_cmd_.clear(); @@ -106,6 +106,7 @@ void IbvsController::reset(const std::vector& 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& q_init) { last_v_camera_visp_.setZero(); } -bool IbvsController::computeInternal(const std::vector& joints_angle, - std::vector& 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(intrinsics.fx); - const double fy = static_cast(intrinsics.fy); - const double cx = static_cast(intrinsics.cx); - const double cy = static_cast(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 I(height, width); - for (int y = 0; y < height; ++y) { - std::memcpy(I[y], gray.ptr(y), static_cast(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 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 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(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(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(std::lround(fx * x_depth_ctrl + cx)); - const int v = static_cast(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 twist_ee; - twist_ee << v_site(0), v_site(1), v_site(2), w_site(0), w_site(1), w_site(2); - - Eigen::Matrix 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 qdot; - const bool ok = dls_solver_->ik( - base_frame_name_, camera_frame_name_, twist_ee_pin, - qdot, mu_, std::numeric_limits::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& joints_angle, double dt, std::vector& q_cmd_out) { @@ -341,19 +133,10 @@ bool IbvsController::compute(const std::vector& 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& joints_angle, return computeInternal(joints_angle, qdot_out); } +bool IbvsController::computeInternal(const std::vector& joints_angle, + std::vector& 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(intr.fx); + const double fy = static_cast(intr.fy); + const double cx = static_cast(intr.cx); + const double cy = static_cast(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(std::lround(fx * x_depth_ctrl + cx)); + const int v = static_cast(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 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 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 qdot; + const bool ok = dls_solver_->ik( + base_frame_name_, camera_frame_name_, twist_urdf, + qdot, mu_, std::numeric_limits::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& q) const { if (!has_joint_position_limits_) return; if (q.size() != static_cast(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_[2].setWorldCoordinates(tag_half_, tag_half_, 0.0); - obj_pts_[3].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_[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) { diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index e408d51d..7a93496a 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/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", diff --git a/cmvr-es/perception/CMakeLists.txt b/cmvr-es/perception/CMakeLists.txt index 6e513c84..b5876789 100644 --- a/cmvr-es/perception/CMakeLists.txt +++ b/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}) diff --git a/cmvr-es/perception/include/apriltag_perception.h b/cmvr-es/perception/include/apriltag_perception.h new file mode 100644 index 00000000..a4486e56 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#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 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& camera = nullptr); + + /** + * @brief 设置相机对象，并清空当前缓存与检测结果。 + * @param camera 相机对象。 + */ + void setCamera(const std::shared_ptr& camera); + const std::shared_ptr& 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& 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 camera_{nullptr}; + + // AprilTag 检测器。 + vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11}; + + // tag 物理边长，单位米。 + double tag_size_m_{0.12}; + + // 与当前内参对应的 ViSP 相机模型。 + vpCameraParameters visp_cam_; + + // 当前帧缓存。 + FrameCache frame_; + + // 当前帧检测到的 tag 列表。 + std::vector tags_; + + // tag id 到 `tags_` 下标的映射。 + std::unordered_map id_to_index_; + + // 最近一次 `update()` 的状态。 + Status last_status_{Status::NO_NEW_FRAME}; +}; + +} // namespace cmvr::perception + +#endif // CMVR_PERCEPTION_APRILTAG_PERCEPTION_H diff --git a/cmvr-es/perception/include/tag_relative_target_3d.h b/cmvr-es/perception/include/tag_relative_target_3d.h index 37fe0434..82cb588d 100644 --- a/cmvr-es/perception/include/tag_relative_target_3d.h +++ b/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 +#include +#include #include #include #include #include #include -#include -#include "devices/camera/abstract_camera.h" +#include "perception/include/apriltag_perception.h" namespace cmvr::perception { /** - * @brief Tag 相对目标 3D 跟踪器（全封装版）。 + * @brief 利用 AprilTag 观测跟踪一个目标点的 3D 位置。 * - * 该类内部完成： - * 1) 从相机抓取 RGB 或 RGBD； - * 2) AprilTag 检测与位姿估计； - * 3) 目标点像素 `(u, v)` 到相机 3D 点的求解； - * 4) 目标点在 tag 坐标系下的锚点缓存与基于 active tag 的跟踪。 + * 坐标系约定： + * - `c`：相机坐标系，与 `AprilTagPerception` 输出的 ViSP 相机坐标系一致。 + * - `t`：AprilTag 自身坐标系。 + * - `p_c_*`：点在相机坐标系 `c` 中的坐标。 + * - `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, ///< 调用成功。 - INVALID_INPUT, ///< 输入参数非法。 - NO_CAMERA, ///< 相机对象为空。 - NO_NEW_FRAME, ///< 未获取到新图像帧。 - BAD_IMAGE, ///< 图像或内参无效。 - NO_TAG, ///< 未检测到 tag。 - NO_OBSERVATION, ///< 当前无可用 tag 观测。 - TARGET_NOT_LOCKED, ///< 尚未建立目标锚点。 - NO_MATCHING_TAG ///< 有观测但没有命中已缓存锚点。 + OK = 0, + INVALID_INPUT, + NO_PERCEPTION, + PERCEPTION_NOT_READY, + NO_TAG, + NO_DEPTH, + BAD_IMAGE, + NO_OBSERVATION, + TARGET_NOT_LOCKED, + NO_MATCHING_TAG }; - /** - * @brief 目标像素转 3D 点的方法。 - */ enum class TargetPointMethod { - TAG_PLANE = 0, ///< 像素射线与 tag 平面求交（多 tag 融合）。 - DEPTH_IMAGE ///< 深度反投影（要求 RGBD 对齐）。 + TAG_PLANE = 0, // 用像素射线与 tag 平面求交恢复目标点。 + DEPTH_IMAGE // 用深度图将像素反投影到相机坐标系。 }; /** - * @brief 构造函数。 - * @param camera 抽象相机对象，可为空。 + * @brief 构造目标点跟踪器。 + * @param perception 共享的感知前端，可为空，后续再通过 `setPerception()` 注入。 */ - explicit TagRelativeTarget3D(const std::shared_ptr& camera = nullptr); - ~TagRelativeTarget3D() = default; + explicit TagRelativeTarget3D(const std::shared_ptr& perception = nullptr); /** - * @brief 设置/替换相机对象。 - * @param camera 抽象相机对象。 + * @brief 设置共享感知前端。 + * @param perception 感知前端。 */ - void setCamera(const std::shared_ptr& camera); + void setPerception(const std::shared_ptr& perception) { perception_ = perception; } + const std::shared_ptr& perception() const { return perception_; } /** - * @brief 清空内部状态（锚点、缓存、追踪状态与最近输出）。 + * @brief 清空所有已锁定的目标锚点与跟踪状态。 */ void clear(); - /** - * @brief 是否没有任何已缓存锚点。 - * @return 没有锚点返回 `true`。 - */ + // 当前是否还没有任何已锁定锚点。 bool empty() const { return anchors_in_tag_.empty(); } - /** - * @brief 当前已缓存锚点数量。 - * @return 锚点数量。 - */ + // 当前已保存的 tag 锚点数量。 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 方法。 - * @param method 求解方法。 + * @brief 设置目标点恢复方式。 + * @param method 目标点恢复方式。 */ void setTargetPointMethod(TargetPointMethod method) { target_point_method_ = method; } - - /** - * @brief 获取当前目标像素转 3D 方法。 - * @return 当前方法。 - */ TargetPointMethod targetPointMethod() const { return target_point_method_; } /** - * @brief 设置 AprilTag 物理边长。 - * @param tag_size_m 边长（单位：米），必须大于 0。 - */ - void setTagSize(double tag_size_m); - - /** - * @brief 设置深度采样与离群剔除参数。 - * - * 该配置仅在 `DEPTH_IMAGE` 模式生效。 - * - * @param window_size_px 深度采样窗口边长（像素，自动修正为奇数）。 - * @param enable_outlier_reject 是否启用离群值剔除。 - * @param outlier_sigma MAD 门限倍数。 - * @param outlier_min_dev_m 离群剔除最小绝对阈值（米）。 + * @brief 设置深度采样配置，仅用于 `TargetPointMethod::DEPTH_IMAGE`。 + * @param window_size_px 深度采样窗口边长，单位像素。 + * @param enable_outlier_reject 是否启用深度离群点剔除。 + * @param outlier_sigma 离群点剔除阈值，单位标准差倍数。 + * @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 切换策略。 - * @param missing_before_switch active tag 连续丢失多少帧后允许切换。 - * @param switch_hysteresis 新候选切换阈值倍率（>1 时更保守）。 + * @brief 设置 active tag 自动切换策略。 + * @param missing_before_switch 当前 active tag 连续丢失多少帧后允许切换。 + * @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_scale_m 距离归一化尺度（米）。 + * @param proximity_power 目标点在 tag 平面内邻近项指数。 + * @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(); - /** - * @brief 当前 active tag id。 - * @return active tag id；无效时为 `-1`。 - */ + // 当前 active tag 的 id；未选中时为 -1。 int activeTagId() const { return active_tag_id_; } - /** - * @brief 当前 active tag 连续丢失帧数。 - * @return 丢失帧计数。 - */ + // 当前 active tag 已连续丢失的帧数。 int activeTagMissingCount() const { return active_tag_missing_count_; } - /** - * @brief 对目标像素进行单次 3D 解算。 - * @param u 像素列坐标。 - * @param v 像素行坐标。 - * @return 成功返回 `true`，结果写入 `last*`。 + * @brief 对单个像素 `(u, v)` 执行一次目标点恢复。 + * @param u 图像列坐标，单位像素。 + * @param v 图像行坐标，单位像素。 + * @return 成功时返回 `true`，结果写入 `lastTargetInCamera()` 等 getter。 */ bool solveFromPixel(int u, int v); /** - * @brief 以目标像素初始化跟踪（解算 + 建锚点 + 选择 active tag）。 - * @param u 像素列坐标。 - * @param v 像素行坐标。 - * @return 成功返回 `true`，结果写入 `last*`。 + * @brief 根据像素 `(u, v)` 锁定目标点，并在当前可见 tag 上建立锚点。 + * @param u 图像列坐标，单位像素。 + * @param v 图像行坐标，单位像素。 + * @return 成功时返回 `true`，并初始化 active tag。 */ bool startTrackingFromPixel(int u, int v); /** - * @brief 进行一次跟踪迭代（active tag 跟踪 + 必要时自动切换）。 - * @return 成功返回 `true`，结果写入 `last*`。 + * @brief 使用当前缓存的 tag 观测跟踪已锁定目标点。 + * @return 成功时返回 `true`，结果写入 `lastTargetInCamera()` 等 getter。 */ bool track(); - /** - * @brief 最近一次成功解算/跟踪得到的目标点相机坐标。 - * @return 目标点 `p_c_target`（米）。 - */ + // 最近一次解算/跟踪得到的目标点在相机坐标系 `c` 中的位置。 const Eigen::Vector3d& lastTargetInCamera() const { return last_p_c_target_; } - /** - * @brief 最近一次结果对应的主导 tag id。 - * @return 平面法为主导 tag id；深度法通常为 `-1`。 - */ + // 最近一次结果实际使用的 tag id；深度锁点时通常为 -1。 int lastUsedTagId() const { return last_used_tag_id_; } - /** - * @brief 最近一次平面法融合离散度。 - * @return 离散度（米）；深度法通常为 0。 - */ + // 最近一次多 tag 融合结果的离散度，单位米。 double lastSpread() const { return last_spread_m_; } - /** - * @brief 最近一次 `track()` 是否发生 active tag 切换。 - * @return 切换返回 `true`。 - */ + // 最近一次 `track()` 是否发生了 active tag 切换。 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 坐标系下的坐标。 - * @return 成功返回 `true`。 + * @param p_t_target_out 输出的目标点坐标，位于 tag 坐标系 `t`，单位米。 + * @return 找到锚点则返回 `true`。 */ bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const; private: - /** - * @brief 单帧 tag 位姿观测（tag 坐标系到相机坐标系）。 - */ - struct TagPoseObservation { - // tag id。 + struct Observation { + // 当前观测对应的 tag id。 int tag_id{-1}; - // tag -> camera 变换矩阵。 - Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; - // 观测权重（检测置信度）。 - double weight{1.0}; - }; - /** - * @brief 最近一帧图像与内参缓存。 - */ - struct FrameCache { - // RGB 图。 - 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}; + // 当前观测的 `T_c_t`，将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。 + Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; + + // 当前观测的权重，通常来自 tag 检测质量。 + double weight{1.0}; }; private: /** - * @brief 重置最近一次调用输出缓存。 + * @brief 清空最近一次解算/跟踪输出缓存。 */ void resetLastOutputs(); /** - * @brief 内部抓帧与观测更新入口。 - * @param need_depth 是否需要深度图。 - * @param need_tags 是否需要更新 tag 观测。 - * @return 成功返回 `true`。 + * @brief 从 `perception_` 同步当前帧缓存。 + * @param need_tags 是否要求当前帧必须含有 tag 观测。 + * @param need_depth 是否要求当前帧必须含有深度图。 + * @return 同步成功且满足要求返回 `true`。 */ - bool updateInternal(bool need_depth, bool need_tags); + bool syncFromPerception(bool need_tags, bool need_depth); /** - * @brief 抓取一帧 RGB 与内参。 - * @return 成功返回 `true`。 + * @brief 用像素射线与当前可见 tag 平面求交，恢复目标点在相机坐标系 `c` 中的位置。 + * @param u 图像列坐标，单位像素。 + * @param v 图像行坐标，单位像素。 + * @return 恢复成功返回 `true`。 */ - bool grabRGB(); + bool solveFromPixelOnPlane(int u, int v); /** - * @brief 抓取一帧 RGBD 与内参。 - * @return 成功返回 `true`。 + * @brief 用深度图将像素 `(u, v)` 反投影到相机坐标系 `c`。 + * @param u 图像列坐标，单位像素。 + * @param v 图像行坐标，单位像素。 + * @return 反投影成功返回 `true`。 */ - bool grabRGBD(); + bool solveFromPixelWithDepth(int u, int v); /** - * @brief 从当前缓存 RGB + 内参中检测 tag 并更新 `observations_`。 - * @return 成功返回 `true`。 + * @brief 用目标点在相机坐标系 `c` 中的位置，给当前可见 tag 建立或补充锚点。 + * @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 锚点缓存。 - * @param p_c_target 目标点在相机坐标系下位置。 - * @param overwrite_existing 是否覆盖已有锚点。 - * @return 成功返回 `true`。 + * @brief 由已存在的锚点恢复当前帧目标点在相机坐标系 `c` 中的位置。 + * @param multi_fuse 为 `true` 时融合所有可见且已建锚点的 tag；否则只使用 active tag。 + * @return 恢复成功返回 `true`。 */ - bool lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing); + bool resolveFromAnchors(bool multi_fuse); /** - * @brief 依据当前观测与锚点恢复目标点相机坐标。 - * @return 成功返回 `true`。 - */ - bool resolveTargetInCameraInternal(); // anchors + observations_ => last_p_c_target_ (+spread/+used) - - /** - * @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`。 + * @brief 根据锁定时求得的目标点初始化锚点和 active tag。 + * @param p_c_lock 锁定时目标点在相机坐标系 `c` 中的位置，单位米。 + * @param used_tag 锁定时实际使用的 tag id。 + * @param spread 锁定时多 tag 融合离散度，单位米。 + * @return 初始化成功返回 `true`。 */ bool initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock, int used_tag, double spread); /** - * @brief 在当前缓存观测下执行 active tag 跟踪。 - * @return 成功返回 `true`。 + * @brief 使用当前 active tag 跟踪目标点，并在需要时切换 active tag。 + * @return 跟踪成功返回 `true`。 */ - bool trackWithActiveTagFromCached(); + bool trackWithActiveTag(); /** - * @brief 判断 4x4 矩阵元素是否全为有限数。 - * @param T 输入矩阵。 - * @return 全有限返回 `true`。 + * @brief 计算某个观测作为 active tag 候选时的评分。 + * @param obs 当前帧某个 tag 的观测。 + * @return 候选评分，越大越优。 + */ + double trackingCandidateScore(const Observation& obs) const; + + /** + * @brief 判断齐次变换矩阵中的所有元素是否为有限数。 + * @param T 齐次变换矩阵。 + * @return 全部为有限数返回 `true`。 */ static bool isFiniteMatrix(const Eigen::Matrix4d& T); /** - * @brief 判断 3D 向量是否全为有限数。 - * @param p 输入向量。 - * @return 全有限返回 `true`。 + * @brief 判断三维点坐标是否为有限数。 + * @param p 三维点。 + * @return 全部为有限数返回 `true`。 */ static bool isFiniteVector(const Eigen::Vector3d& p); /** - * @brief 点从 tag 坐标系变换到相机坐标系。 - * @param T_c_t tag 到相机变换。 - * @param p_t tag 坐标系下点。 - * @return 相机坐标系下点。 + * @brief 将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。 + * @param T_c_t 齐次变换，表示 `t -> c`。 + * @param p_t 点在 tag 坐标系 `t` 中的坐标，单位米。 + * @return 点在相机坐标系 `c` 中的坐标，单位米。 */ static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t); /** - * @brief 点从相机坐标系变换到 tag 坐标系。 - * @param T_c_t tag 到相机变换。 - * @param p_c 相机坐标系下点。 - * @return tag 坐标系下点。 + * @brief 将相机坐标系 `c` 中的点变换到 tag 坐标系 `t`。 + * @param T_c_t 齐次变换，表示 `t -> c`。 + * @param p_c 点在相机坐标系 `c` 中的坐标，单位米。 + * @return 点在 tag 坐标系 `t` 中的坐标，单位米。 */ static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c); /** - * @brief 相机射线与 tag 平面求交。 - * @param T_c_t tag 到相机变换。 - * @param ray_c 相机坐标系下射线方向。 - * @param p_c_intersection 输出交点（相机坐标系）。 - * @param view_cos_out 可选输出：视线与平面法向夹角余弦绝对值。 + * @brief 计算相机像素射线与 tag 平面的交点。 + * @param T_c_t 齐次变换，表示 `t -> c`。 + * @param ray_c 像素射线方向，位于相机坐标系 `c`。 + * @param p_c_intersection 输出交点，位于相机坐标系 `c`，单位米。 + * @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 归一化观测权重（非法权重回退到默认值）。 - * @param w 输入权重。 - * @return 处理后的权重。 + * @brief 将观测权重清洗为合法的正数。 + * @param w 原始权重。 + * @return 清洗后的权重。 */ - static double sanitizeObsWeight(double w); - - /** - * @brief 计算单个观测作为 active tag 候选的评分。 - * @param obs tag 观测。 - * @return 候选评分。 - */ - double trackingCandidateScore(const TagPoseObservation& obs) const; + static double sanitizeWeight(double w); private: - // 目标锚点缓存：tag_id -> p_t_target。 + // 共享感知前端。 + std::shared_ptr perception_{nullptr}; + + // 当前使用的感知帧 id。 + uint64_t used_frame_id_{0}; + + // 当前帧相机内参。 + device::Rs2Intrinsics intr_{}; + + // 指向当前帧深度图的只读引用。 + const cv::Mat* depth_{nullptr}; + + // 当前帧 tag 观测列表。 + std::vector observations_; + + // 目标点锚点表：`tag_id -> p_t_target`，其中 `p_t_target` 位于对应 tag 坐标系 `t`。 std::unordered_map anchors_in_tag_; - // 相机对象。 - std::shared_ptr 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 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}; }; diff --git a/cmvr-es/perception/src/apriltag_perception.cpp b/cmvr-es/perception/src/apriltag_perception.cpp new file mode 100644 index 00000000..a084db92 --- /dev/null +++ b/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 +#include +#include + +#include + +namespace cmvr::perception { + +AprilTagPerception::AprilTagPerception(const std::shared_ptr& camera) + : camera_(camera) { + detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); +} + +void AprilTagPerception::setCamera(const std::shared_ptr& 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(frame_.stream.intrinsics.fx); + const double fy = static_cast(frame_.stream.intrinsics.fy); + const double cx = static_cast(frame_.stream.intrinsics.cx); + const double cy = static_cast(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(y), + static_cast(w)); + } + return true; +} + +bool AprilTagPerception::detectTags() { + std::vector 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 tag_ids = detector_.getTagsId(); + const std::vector 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(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 diff --git a/cmvr-es/perception/src/tag_relative_target_3d.cpp b/cmvr-es/perception/src/tag_relative_target_3d.cpp index 846052fc..300fe73d 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d.cpp +++ b/cmvr-es/perception/src/tag_relative_target_3d.cpp @@ -2,26 +2,14 @@ #include #include -#include #include #include -#include -#include -#include -#include - #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& camera) - : camera_(camera) { - detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); -} +TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr& perception) + : perception_(perception) {} -void TagRelativeTarget3D::setCamera(const std::shared_ptr& camera) { - camera_ = camera; - frame_ = FrameCache{}; - observations_.clear(); - obs_frame_id_ = 0; +void TagRelativeTarget3D::clear() { 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_NEW_FRAME: return "no_new_frame"; - case Status::BAD_IMAGE: return "bad_image"; + case Status::NO_PERCEPTION: return "no_perception"; + case Status::PERCEPTION_NOT_READY: return "perception_not_ready"; 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; } -// ========================= -// Internal frame grabbing -// ========================= -bool TagRelativeTarget3D::grabRGB() { - if (!camera_) { - last_status_ = Status::NO_CAMERA; - return false; - } +void TagRelativeTarget3D::resetLastOutputs() { + last_p_c_target_.setZero(); + last_used_tag_id_ = -1; + last_spread_m_ = 0.0; + last_switched_ = 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 { - camera_->getRGBImage(frame_.color, frame_.intr); - } catch (...) { - last_status_ = Status::NO_NEW_FRAME; - 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++; +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; } -bool TagRelativeTarget3D::grabRGBD() { - if (!camera_) { - last_status_ = Status::NO_CAMERA; +bool TagRelativeTarget3D::syncFromPerception(bool need_tags, bool need_depth) { + if (!perception_) { + last_status_ = Status::NO_PERCEPTION; return false; } - frame_.has_color = frame_.has_depth = frame_.has_intr = false; - - try { - camera_->getRGBDImages(frame_.color, frame_.depth, frame_.intr); - } catch (...) { - last_status_ = Status::NO_NEW_FRAME; + // perception.update() 失败时，statusToString(perception->lastStatus()) 可用于上层统一打印 + // 这里我们只根据缓存是否存在来决定是否能继续。 + const auto& color = perception_->color(); + if (color.empty()) { + last_status_ = Status::PERCEPTION_NOT_READY; return false; } - if (frame_.color.empty() || frame_.depth.empty()) { - last_status_ = Status::NO_NEW_FRAME; - return false; - } + intr_ = perception_->intrinsics(); + depth_ = &perception_->depth(); + 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; - } else { - if (!grabRGB()) return false; + if (!perception_->hasDepth() || depth_->empty()) { + last_status_ = Status::NO_DEPTH; + return false; + } } if (need_tags) { - if (!updateObservations()) return false; - } else { - last_status_ = Status::OK; + if (!perception_->hasTags()) { + last_status_ = Status::NO_TAG; + 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(frame_.intr.fx); - const double fy = static_cast(frame_.intr.fy); - const double cx = static_cast(frame_.intr.cx); - const double cy = static_cast(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 I(height, width); - for (int y = 0; y < height; ++y) { - std::memcpy(I[y], gray.ptr(y), static_cast(width)); - } - - std::vector 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 tag_ids = detector_.getTagsId(); - const std::vector 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(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 - return solveFromPixelWithDepthCached(u, v); + if (!syncFromPerception(false, true)) return false; + return solveFromPixelWithDepth(u, v); } - if (!updateInternal(false, true)) return false; // RGB + tags - return solveFromPixelOnPlaneCached(u, v); + if (!syncFromPerception(true, false)) return false; + 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; - // 深度法：used_tag=-1, spread=0（已经在 solveFromPixelWithDepthCached 里写好） + if (!solveFromPixelWithDepth(u, v)) return false; + // 深度法：last_used_tag_id_=-1，last_spread_m_=0 } else { - if (!solveFromPixelOnPlaneCached(u, v)) return false; + if (!solveFromPixelOnPlane(u, v)) return false; } - // 用 lock 点建锚点 + 选 active - const Eigen::Vector3d p_lock = last_p_c_target_; - const int used_tag = last_used_tag_id_; - const double spread = last_spread_m_; - return initTrackingFromResolvedPoint(p_lock, used_tag, spread); + // 2) 建锚点 + 选 active + return initTrackingFromResolvedPoint(last_p_c_target_, last_used_tag_id_, last_spread_m_); } 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 trackWithActiveTagFromCached(); + return trackWithActiveTag(); } -// ========================= -// 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) { +bool TagRelativeTarget3D::solveFromPixelOnPlane(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) { - if (!frame_.has_depth || frame_.depth.empty() || !frame_.has_intr) { - last_status_ = Status::BAD_IMAGE; +bool TagRelativeTarget3D::solveFromPixelWithDepth(int u, int v) { + if (!depth_ || depth_->empty()) { + last_status_ = Status::NO_DEPTH; return false; } Eigen::Vector3d p = Eigen::Vector3d::Zero(); - if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(frame_.intr, - frame_.depth, + if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(intr_, + *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, - int used_tag, - double spread) { - if (!isFiniteVector(p_c_lock)) { +bool TagRelativeTarget3D::lockAnchorsFromPoint(const Eigen::Vector3d& p_c_target, bool overwrite_existing) { + if (!isFiniteVector(p_c_target)) { last_status_ = Status::INVALID_INPUT; return false; } @@ -541,10 +317,30 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p return false; } - // 建锚点（覆盖） - if (!lockTargetInCameraInternal(p_c_lock, true)) 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::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::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() { - if (observations_.empty()) { - last_status_ = Status::NO_OBSERVATION; - return false; - } - +bool TagRelativeTarget3D::trackWithActiveTag() { int active_obs_index = -1; double active_score = -std::numeric_limits::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; } -// ========================= -// 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 { +double TagRelativeTarget3D::trackingCandidateScore(const Observation& obs) const { if (obs.tag_id < 0) return -std::numeric_limits::infinity(); - const auto it = anchors_in_tag_.find(obs.tag_id); if (it == anchors_in_tag_.end()) return -std::numeric_limits::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, - Eigen::Vector3d& p_c_intersection, - double* view_cos_out) { + const Eigen::Vector3d& ray_c, + Eigen::Vector3d& p_c_intersection, + 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; diff --git a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp b/cmvr-es/perception/src/tag_relative_target_3d_test.cpp index ff747f1a..6d6c8f1b 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp +++ b/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_align_mode(cmvr::config::ALIGN_MODE_COLOR); + 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(cam_cfg); - cmvr::perception::TagRelativeTarget3D tracker(camera); - tracker.setTagSize(tag_size); + + auto perception = std::make_shared(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) { - const std::string serial = kRsSerial; - if (serial.empty()) { - GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp"; - } - - cmvr::config::RealSenseCameraConfig cam_cfg; - cam_cfg.set_id("tag_relative_target_3d_test_no_display"); - cam_cfg.set_serialnumber(serial); - cam_cfg.set_width(kWidth); - cam_cfg.set_height(kHeight); - cam_cfg.set_fps(kFps); - 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(cam_cfg); - cmvr::perception::TagRelativeTarget3D tracker(camera); - ASSERT_NO_THROW(camera->init()); - ASSERT_NO_THROW(camera->start()); - struct CameraStopGuard { - std::shared_ptr cam; - ~CameraStopGuard() { - if (!cam) return; - try { - cam->stop(); - } catch (...) { - } - } - } stop_guard{camera}; - tracker.setTagSize(kTagSize); - tracker.setActiveTagSwitchPolicy(4, 1.2); - tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); - // 深度法采样：5x5 中值 + MAD 离群剔除。 - tracker.setDepthSamplingConfig(5, true, 2.5, 0.003); - const int tries = kTries; - const bool infinite = (tries <= 0); - int plane_fail_count = 0; - int depth_fail_count = 0; - int plane_ok_count = 0; - int depth_ok_count = 0; - int both_ok_count = 0; - int printed_count = 0; - bool got_target = false; - const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2); - const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2); - - for (int i = 0; infinite || i < tries; ++i) { - // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。 - tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); - const bool plane_ok = tracker.solveFromPixel(target_u, target_v); - const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); - const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1; - const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0; - if (plane_ok) { - ++plane_ok_count; - } else { - ++plane_fail_count; - } - - tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE); - const bool depth_ok = tracker.solveFromPixel(target_u, target_v); - const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); - if (depth_ok) { - ++depth_ok_count; - } else { - ++depth_fail_count; - } - - if (plane_ok && depth_ok) { - ++both_ok_count; - } - if (plane_ok || depth_ok) { - got_target = true; - } - - std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] " - << "uv=[" << target_u << "," << target_v << "] " - << "plane_ok=" << plane_ok - << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]") - << " used_tag=" << plane_used_tag - << " spread=" << plane_spread - << " | depth_ok=" << depth_ok - << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]"); - if (plane_ok && depth_ok) { - std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm(); - } - std::cout << std::endl; - - ++printed_count; - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - } - - EXPECT_TRUE(got_target) - << "Failed to solve target point in camera frame. " - << " plane_fail=" << plane_fail_count - << " depth_fail=" << depth_fail_count - << " plane_ok=" << plane_ok_count - << " depth_ok=" << depth_ok_count - << " both_ok=" << both_ok_count - << " printed=" << printed_count; -} +// TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) { +// const std::string serial = kRsSerial; +// if (serial.empty()) { +// GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp"; +// } +// +// cmvr::config::RealSenseCameraConfig cam_cfg; +// cam_cfg.set_id("tag_relative_target_3d_test_no_display"); +// cam_cfg.set_serialnumber(serial); +// cam_cfg.set_width(kWidth); +// cam_cfg.set_height(kHeight); +// cam_cfg.set_fps(kFps); +// 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(cam_cfg); +// cmvr::perception::TagRelativeTarget3D tracker(camera); +// ASSERT_NO_THROW(camera->init()); +// ASSERT_NO_THROW(camera->start()); +// struct CameraStopGuard { +// std::shared_ptr cam; +// ~CameraStopGuard() { +// if (!cam) return; +// try { +// cam->stop(); +// } catch (...) { +// } +// } +// } stop_guard{camera}; +// tracker.setTagSize(kTagSize); +// tracker.setActiveTagSwitchPolicy(4, 1.2); +// tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); +// // 深度法采样：5x5 中值 + MAD 离群剔除。 +// tracker.setDepthSamplingConfig(5, true, 2.5, 0.003); +// const int tries = kTries; +// const bool infinite = (tries <= 0); +// int plane_fail_count = 0; +// int depth_fail_count = 0; +// int plane_ok_count = 0; +// int depth_ok_count = 0; +// int both_ok_count = 0; +// int printed_count = 0; +// bool got_target = false; +// const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2); +// const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2); +// +// for (int i = 0; infinite || i < tries; ++i) { +// // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。 +// tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); +// const bool plane_ok = tracker.solveFromPixel(target_u, target_v); +// const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); +// const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1; +// const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0; +// if (plane_ok) { +// ++plane_ok_count; +// } else { +// ++plane_fail_count; +// } +// +// tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE); +// const bool depth_ok = tracker.solveFromPixel(target_u, target_v); +// const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); +// if (depth_ok) { +// ++depth_ok_count; +// } else { +// ++depth_fail_count; +// } +// +// if (plane_ok && depth_ok) { +// ++both_ok_count; +// } +// if (plane_ok || depth_ok) { +// got_target = true; +// } +// +// std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] " +// << "uv=[" << target_u << "," << target_v << "] " +// << "plane_ok=" << plane_ok +// << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]") +// << " used_tag=" << plane_used_tag +// << " spread=" << plane_spread +// << " | depth_ok=" << depth_ok +// << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]"); +// if (plane_ok && depth_ok) { +// std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm(); +// } +// std::cout << std::endl; +// +// ++printed_count; +// std::this_thread::sleep_for(std::chrono::milliseconds(30)); +// } +// +// EXPECT_TRUE(got_target) +// << "Failed to solve target point in camera frame. " +// << " plane_fail=" << plane_fail_count +// << " depth_fail=" << depth_fail_count +// << " plane_ok=" << plane_ok_count +// << " depth_ok=" << depth_ok_count +// << " both_ok=" << both_ok_count +// << " printed=" << printed_count; +// } diff --git a/model/xiaoyan_description/dual_arm.xml b/model/xiaoyan_description/dual_arm.xml index 12ec88e5..4460b2a1 100644 --- a/model/xiaoyan_description/dual_arm.xml +++ b/model/xiaoyan_description/dual_arm.xml @@ -66,7 +66,7 @@ - +