refactor: optimize IbvsController&TagRelativeTarget3D
This commit is contained in:
parent
29e5c8833c
commit
777416d73b
@ -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)
|
||||
|
||||
@ -18,6 +18,9 @@ target_include_directories(common PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(common PUBLIC
|
||||
cmvr_es::proto
|
||||
opencv_core
|
||||
opencv_imgproc
|
||||
opencv_highgui
|
||||
avformat
|
||||
avdevice
|
||||
avutil
|
||||
@ -26,4 +29,20 @@ target_link_libraries(common PUBLIC
|
||||
)
|
||||
|
||||
add_library(cmvr_es::common ALIAS common)
|
||||
install(TARGETS common LIBRARY DESTINATION lib)
|
||||
install(TARGETS common LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(image_display_test
|
||||
utils/visualization/image_display_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(image_display_test
|
||||
PRIVATE
|
||||
cmvr_es::common
|
||||
cmvr_es::perception
|
||||
cmvr_es::device::realsense_camera
|
||||
cmvr_es::proto
|
||||
glog
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
)
|
||||
|
||||
609
cmvr-es/common/utils/visualization/image_display.h
Normal file
609
cmvr-es/common/utils/visualization/image_display.h
Normal file
@ -0,0 +1,609 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace cmvr::common {
|
||||
|
||||
/**
|
||||
* @brief 图像像素坐标。
|
||||
*
|
||||
* 坐标系约定：
|
||||
* - `u`：图像列坐标，向右为正，单位像素。
|
||||
* - `v`：图像行坐标，向下为正，单位像素。
|
||||
*/
|
||||
struct Pixel {
|
||||
int u{0}; // 图像像素列坐标，单位像素。
|
||||
int v{0}; // 图像像素行坐标，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief BGR 颜色定义。
|
||||
*/
|
||||
struct Color {
|
||||
uint8_t b{0}; // 蓝色通道，范围 [0, 255]。
|
||||
uint8_t g{255}; // 绿色通道，范围 [0, 255]。
|
||||
uint8_t r{0}; // 红色通道，范围 [0, 255]。
|
||||
|
||||
/**
|
||||
* @brief 转为 OpenCV 的 `cv::Scalar`。
|
||||
*/
|
||||
cv::Scalar toCvScalar() const {
|
||||
return cv::Scalar(static_cast<double>(b),
|
||||
static_cast<double>(g),
|
||||
static_cast<double>(r));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 预定义红色。
|
||||
*/
|
||||
static Color red() { return Color{0, 0, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义绿色。
|
||||
*/
|
||||
static Color green() { return Color{0, 255, 0}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义蓝色。
|
||||
*/
|
||||
static Color blue() { return Color{255, 0, 0}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义黄色。
|
||||
*/
|
||||
static Color yellow() { return Color{0, 255, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义白色。
|
||||
*/
|
||||
static Color white() { return Color{255, 255, 255}; }
|
||||
|
||||
/**
|
||||
* @brief 预定义黑色。
|
||||
*/
|
||||
static Color black() { return Color{0, 0, 0}; }
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的文本叠加配置。
|
||||
*/
|
||||
struct TextOverlay {
|
||||
std::string text; // 需要显示的字符串内容。
|
||||
Pixel position_px{}; // 文本左下角在图像像素坐标系中的位置。
|
||||
Color color{Color::green()}; // 文本颜色。
|
||||
double font_scale{0.7}; // OpenCV 字体缩放系数。
|
||||
int thickness{2}; // 文本线宽，单位像素。
|
||||
int font_face{cv::FONT_HERSHEY_SIMPLEX}; // OpenCV 字体类型。
|
||||
bool draw_background{false}; // 是否绘制文本背景框。
|
||||
Color background_color{Color::black()}; // 文本背景框颜色。
|
||||
int background_padding_px{2}; // 文本背景框四周留白，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的点叠加配置。
|
||||
*/
|
||||
struct PointOverlay {
|
||||
Pixel position_px{}; // 点中心在图像像素坐标系中的位置。
|
||||
Color color{Color::red()}; // 点颜色。
|
||||
int radius_px{5}; // 圆点半径，单位像素。
|
||||
int thickness{2}; // 线宽，`-1` 表示填充。
|
||||
std::string label; // 点旁边附带显示的字符串；为空时不显示。
|
||||
Pixel label_offset_px{8, -8}; // 标签相对点中心的像素偏移。
|
||||
double label_font_scale{0.6}; // 点标签字体缩放系数。
|
||||
int label_thickness{2}; // 点标签线宽，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的圆圈叠加配置。
|
||||
*/
|
||||
struct CircleOverlay {
|
||||
Pixel center_px{}; // 圆心在图像像素坐标系中的位置。
|
||||
int radius_px{10}; // 圆半径，单位像素。
|
||||
Color color{Color::yellow()}; // 圆圈颜色。
|
||||
int thickness{2}; // 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 图像上的十字叠加配置。
|
||||
*/
|
||||
struct CrossOverlay {
|
||||
Pixel center_px{}; // 十字中心在图像像素坐标系中的位置。
|
||||
int arm_length_px{8}; // 单侧十字臂长度，单位像素。
|
||||
Color color{Color::blue()}; // 十字颜色。
|
||||
int thickness{2}; // 十字线宽，单位像素。
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 通用图像叠加显示接口。
|
||||
*
|
||||
* 该接口不依赖任何感知或跟踪类，只抽象三类输入：
|
||||
* - 待显示的图像数据；
|
||||
* - 文本叠加项；
|
||||
* - 点叠加项。
|
||||
*/
|
||||
class ImageDisplay {
|
||||
public:
|
||||
virtual ~ImageDisplay() = default;
|
||||
|
||||
/**
|
||||
* @brief 设置窗口名称。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
virtual void setWindowName(const std::string& window_name) = 0;
|
||||
|
||||
/**
|
||||
* @brief 设置待显示的图像。
|
||||
* @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
|
||||
*/
|
||||
virtual void setImage(const cv::Mat& image) = 0;
|
||||
|
||||
/**
|
||||
* @brief 清空所有叠加项，但保留当前图像。
|
||||
*/
|
||||
virtual void clearOverlays() = 0;
|
||||
|
||||
/**
|
||||
* @brief 添加文本叠加项。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
virtual void showText(const TextOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、字符串、颜色和大小直接添加文本。
|
||||
* @param text 需要显示的字符串内容。
|
||||
* @param position_px 文本左下角在图像像素坐标系中的位置。
|
||||
* @param color 文本颜色。
|
||||
* @param font_scale OpenCV 字体缩放系数。
|
||||
* @param thickness 文本线宽，单位像素。
|
||||
*/
|
||||
virtual void showText(const std::string& text,
|
||||
const Pixel& position_px,
|
||||
const Color& color,
|
||||
double font_scale = 0.7,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 添加点叠加项。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
virtual void showPoint(const PointOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、颜色和大小直接添加点。
|
||||
* @param position_px 点中心在图像像素坐标系中的位置。
|
||||
* @param color 点颜色。
|
||||
* @param radius_px 圆点半径，单位像素。
|
||||
* @param thickness 线宽，`-1` 表示填充。
|
||||
* @param label 点旁边附带显示的字符串；为空时不显示。
|
||||
*/
|
||||
virtual void showPoint(const Pixel& position_px,
|
||||
const Color& color,
|
||||
int radius_px = 5,
|
||||
int thickness = 2,
|
||||
const std::string& label = {}) = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示圆圈叠加项。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
virtual void showCircle(const CircleOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、半径、颜色和线宽直接显示圆圈。
|
||||
* @param center_px 圆心在图像像素坐标系中的位置。
|
||||
* @param radius_px 圆半径，单位像素。
|
||||
* @param color 圆圈颜色。
|
||||
* @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
*/
|
||||
virtual void showCircle(const Pixel& center_px,
|
||||
int radius_px,
|
||||
const Color& color,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示十字叠加项。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
virtual void showCross(const CrossOverlay& overlay) = 0;
|
||||
|
||||
/**
|
||||
* @brief 按位置、尺寸、颜色和线宽直接显示十字。
|
||||
* @param center_px 十字中心在图像像素坐标系中的位置。
|
||||
* @param arm_length_px 单侧十字臂长度，单位像素。
|
||||
* @param color 十字颜色。
|
||||
* @param thickness 十字线宽，单位像素。
|
||||
*/
|
||||
virtual void showCross(const Pixel& center_px,
|
||||
int arm_length_px,
|
||||
const Color& color,
|
||||
int thickness = 2) = 0;
|
||||
|
||||
/**
|
||||
* @brief 判断当前是否已有待显示图像。
|
||||
*/
|
||||
virtual bool hasImage() const = 0;
|
||||
|
||||
/**
|
||||
* @brief 将当前图像和叠加项渲染到输出图像。
|
||||
* @param rendered_image 输出的渲染结果图像，BGR 三通道。
|
||||
* @return 渲染成功返回 `true`。
|
||||
*/
|
||||
virtual bool render(cv::Mat& rendered_image) const = 0;
|
||||
|
||||
/**
|
||||
* @brief 显示当前渲染结果。
|
||||
* @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
|
||||
* @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
|
||||
*/
|
||||
virtual int show(int wait_key_ms = 1) = 0;
|
||||
|
||||
/**
|
||||
* @brief 关闭显示窗口。
|
||||
*/
|
||||
virtual void close() = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 基于 OpenCV HighGUI 的图像叠加显示实现。
|
||||
*/
|
||||
class OpenCvImageCvDisplay final : public ImageDisplay {
|
||||
public:
|
||||
/**
|
||||
* @brief 构造显示器。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
explicit OpenCvImageCvDisplay(std::string window_name = "image_overlay")
|
||||
: window_name_(std::move(window_name)) {}
|
||||
|
||||
/**
|
||||
* @brief 设置窗口名称。
|
||||
* @param window_name 显示窗口名称。
|
||||
*/
|
||||
void setWindowName(const std::string& window_name) override {
|
||||
window_name_ = window_name;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置待显示图像。
|
||||
* @param image 输入图像，支持灰度图、BGR 图或 BGRA 图。
|
||||
*/
|
||||
void setImage(const cv::Mat& image) override {
|
||||
image_ = image.clone();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 清空所有叠加项，但保留当前图像。
|
||||
*/
|
||||
void clearOverlays() override {
|
||||
texts_.clear();
|
||||
points_.clear();
|
||||
circles_.clear();
|
||||
crosses_.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加文本叠加项。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
void showText(const TextOverlay& overlay) override {
|
||||
texts_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、字符串、颜色和大小直接添加文本。
|
||||
* @param text 需要显示的字符串内容。
|
||||
* @param position_px 文本左下角在图像像素坐标系中的位置。
|
||||
* @param color 文本颜色。
|
||||
* @param font_scale OpenCV 字体缩放系数。
|
||||
* @param thickness 文本线宽，单位像素。
|
||||
*/
|
||||
void showText(const std::string& text,
|
||||
const Pixel& position_px,
|
||||
const Color& color,
|
||||
double font_scale = 0.7,
|
||||
int thickness = 2) override {
|
||||
TextOverlay overlay;
|
||||
overlay.text = text;
|
||||
overlay.position_px = position_px;
|
||||
overlay.color = color;
|
||||
overlay.font_scale = font_scale;
|
||||
overlay.thickness = thickness;
|
||||
showText(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 添加点叠加项。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
void showPoint(const PointOverlay& overlay) override {
|
||||
points_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、颜色和大小直接添加点。
|
||||
* @param position_px 点中心在图像像素坐标系中的位置。
|
||||
* @param color 点颜色。
|
||||
* @param radius_px 圆点半径，单位像素。
|
||||
* @param thickness 线宽，`-1` 表示填充。
|
||||
* @param label 点旁边附带显示的字符串；为空时不显示。
|
||||
*/
|
||||
void showPoint(const Pixel& position_px,
|
||||
const Color& color,
|
||||
int radius_px = 5,
|
||||
int thickness = 2,
|
||||
const std::string& label = {}) override {
|
||||
PointOverlay overlay;
|
||||
overlay.position_px = position_px;
|
||||
overlay.color = color;
|
||||
overlay.radius_px = radius_px;
|
||||
overlay.thickness = thickness;
|
||||
overlay.label = label;
|
||||
showPoint(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示圆圈叠加项。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
void showCircle(const CircleOverlay& overlay) override {
|
||||
circles_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、半径、颜色和线宽直接显示圆圈。
|
||||
* @param center_px 圆心在图像像素坐标系中的位置。
|
||||
* @param radius_px 圆半径，单位像素。
|
||||
* @param color 圆圈颜色。
|
||||
* @param thickness 圆圈线宽，单位像素，`-1` 表示填充。
|
||||
*/
|
||||
void showCircle(const Pixel& center_px,
|
||||
int radius_px,
|
||||
const Color& color,
|
||||
int thickness = 2) override {
|
||||
CircleOverlay overlay;
|
||||
overlay.center_px = center_px;
|
||||
overlay.radius_px = radius_px;
|
||||
overlay.color = color;
|
||||
overlay.thickness = thickness;
|
||||
showCircle(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示十字叠加项。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
void showCross(const CrossOverlay& overlay) override {
|
||||
crosses_.push_back(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 按位置、尺寸、颜色和线宽直接显示十字。
|
||||
* @param center_px 十字中心在图像像素坐标系中的位置。
|
||||
* @param arm_length_px 单侧十字臂长度，单位像素。
|
||||
* @param color 十字颜色。
|
||||
* @param thickness 十字线宽，单位像素。
|
||||
*/
|
||||
void showCross(const Pixel& center_px,
|
||||
int arm_length_px,
|
||||
const Color& color,
|
||||
int thickness = 2) override {
|
||||
CrossOverlay overlay;
|
||||
overlay.center_px = center_px;
|
||||
overlay.arm_length_px = arm_length_px;
|
||||
overlay.color = color;
|
||||
overlay.thickness = thickness;
|
||||
showCross(overlay);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 判断当前是否已有待显示图像。
|
||||
*/
|
||||
bool hasImage() const override {
|
||||
return !image_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将当前图像和叠加项渲染到输出图像。
|
||||
* @param rendered_image 输出的渲染结果图像，BGR 三通道。
|
||||
* @return 渲染成功返回 `true`。
|
||||
*/
|
||||
bool render(cv::Mat& rendered_image) const override {
|
||||
if (!toBgrImage(image_, rendered_image)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (const CircleOverlay& overlay : circles_) {
|
||||
drawCircle(rendered_image, overlay);
|
||||
}
|
||||
for (const CrossOverlay& overlay : crosses_) {
|
||||
drawCross(rendered_image, overlay);
|
||||
}
|
||||
for (const PointOverlay& overlay : points_) {
|
||||
drawPoint(rendered_image, overlay);
|
||||
}
|
||||
for (const TextOverlay& overlay : texts_) {
|
||||
drawText(rendered_image, overlay);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 显示当前渲染结果。
|
||||
* @param wait_key_ms `cv::waitKey` 等待时间，单位毫秒。
|
||||
* @return 返回 `cv::waitKey` 的按键值；若没有可显示图像则返回 `-1`。
|
||||
*/
|
||||
int show(int wait_key_ms = 1) override {
|
||||
cv::Mat rendered_image;
|
||||
if (!render(rendered_image)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!window_created_) {
|
||||
cv::namedWindow(window_name_, cv::WINDOW_NORMAL);
|
||||
window_created_ = true;
|
||||
}
|
||||
cv::imshow(window_name_, rendered_image);
|
||||
return cv::waitKey(wait_key_ms);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 关闭显示窗口。
|
||||
*/
|
||||
void close() override {
|
||||
if (!window_created_) {
|
||||
return;
|
||||
}
|
||||
cv::destroyWindow(window_name_);
|
||||
window_created_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 将输入图像转换为可叠加绘制的 BGR 三通道图像。
|
||||
* @param input 输入图像。
|
||||
* @param output 输出 BGR 图像。
|
||||
* @return 转换成功返回 `true`。
|
||||
*/
|
||||
static bool toBgrImage(const cv::Mat& input, cv::Mat& output) {
|
||||
if (input.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (input.channels() == 1) {
|
||||
cv::cvtColor(input, output, cv::COLOR_GRAY2BGR);
|
||||
return true;
|
||||
}
|
||||
if (input.channels() == 3) {
|
||||
output = input.clone();
|
||||
return true;
|
||||
}
|
||||
if (input.channels() == 4) {
|
||||
cv::cvtColor(input, output, cv::COLOR_BGRA2BGR);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制文本叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 文本叠加配置。
|
||||
*/
|
||||
static void drawText(cv::Mat& image, const TextOverlay& overlay) {
|
||||
const cv::Point origin(overlay.position_px.u, overlay.position_px.v);
|
||||
const int thickness = std::max(1, overlay.thickness);
|
||||
const double font_scale = std::max(0.0, overlay.font_scale);
|
||||
|
||||
if (overlay.draw_background) {
|
||||
int baseline = 0;
|
||||
const cv::Size text_size = cv::getTextSize(overlay.text,
|
||||
overlay.font_face,
|
||||
font_scale,
|
||||
thickness,
|
||||
&baseline);
|
||||
const int pad = std::max(0, overlay.background_padding_px);
|
||||
const cv::Point top_left(origin.x - pad, origin.y - text_size.height - pad);
|
||||
const cv::Point bottom_right(origin.x + text_size.width + pad, origin.y + baseline + pad);
|
||||
cv::rectangle(image,
|
||||
top_left,
|
||||
bottom_right,
|
||||
overlay.background_color.toCvScalar(),
|
||||
cv::FILLED);
|
||||
}
|
||||
|
||||
cv::putText(image,
|
||||
overlay.text,
|
||||
origin,
|
||||
overlay.font_face,
|
||||
font_scale,
|
||||
overlay.color.toCvScalar(),
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制点叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 点叠加配置。
|
||||
*/
|
||||
static void drawPoint(cv::Mat& image, const PointOverlay& overlay) {
|
||||
const cv::Point center(overlay.position_px.u, overlay.position_px.v);
|
||||
const int radius = std::max(1, overlay.radius_px);
|
||||
cv::circle(image,
|
||||
center,
|
||||
radius,
|
||||
overlay.color.toCvScalar(),
|
||||
overlay.thickness,
|
||||
cv::LINE_AA);
|
||||
|
||||
if (!overlay.label.empty()) {
|
||||
TextOverlay label_overlay;
|
||||
label_overlay.text = overlay.label;
|
||||
label_overlay.position_px = Pixel{center.x + overlay.label_offset_px.u,
|
||||
center.y + overlay.label_offset_px.v};
|
||||
label_overlay.color = overlay.color;
|
||||
label_overlay.font_scale = overlay.label_font_scale;
|
||||
label_overlay.thickness = overlay.label_thickness;
|
||||
drawText(image, label_overlay);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制圆圈叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 圆圈叠加配置。
|
||||
*/
|
||||
static void drawCircle(cv::Mat& image, const CircleOverlay& overlay) {
|
||||
const cv::Point center(overlay.center_px.u, overlay.center_px.v);
|
||||
const int radius = std::max(1, overlay.radius_px);
|
||||
const int thickness = (overlay.thickness == 0) ? 1 : overlay.thickness;
|
||||
cv::circle(image,
|
||||
center,
|
||||
radius,
|
||||
overlay.color.toCvScalar(),
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在图像上绘制十字叠加项。
|
||||
* @param image 目标图像，BGR 三通道。
|
||||
* @param overlay 十字叠加配置。
|
||||
*/
|
||||
static void drawCross(cv::Mat& image, const CrossOverlay& overlay) {
|
||||
const cv::Point center(overlay.center_px.u, overlay.center_px.v);
|
||||
const int arm_length = std::max(1, overlay.arm_length_px);
|
||||
const int thickness = std::max(1, overlay.thickness);
|
||||
const cv::Scalar color = overlay.color.toCvScalar();
|
||||
cv::line(image,
|
||||
cv::Point(center.x - arm_length, center.y),
|
||||
cv::Point(center.x + arm_length, center.y),
|
||||
color,
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
cv::line(image,
|
||||
cv::Point(center.x, center.y - arm_length),
|
||||
cv::Point(center.x, center.y + arm_length),
|
||||
color,
|
||||
thickness,
|
||||
cv::LINE_AA);
|
||||
}
|
||||
|
||||
std::string window_name_; // 显示窗口名称。
|
||||
cv::Mat image_; // 当前待显示图像。
|
||||
std::vector<TextOverlay> texts_; // 当前待绘制的文本叠加列表。
|
||||
std::vector<PointOverlay> points_; // 当前待绘制的点叠加列表。
|
||||
std::vector<CircleOverlay> circles_; // 当前待绘制的圆圈叠加列表。
|
||||
std::vector<CrossOverlay> crosses_; // 当前待绘制的十字叠加列表。
|
||||
bool window_created_{false}; // 显示窗口是否已经创建。
|
||||
};
|
||||
|
||||
} // namespace cmvr::common
|
||||
373
cmvr-es/common/utils/visualization/image_display_test.cpp
Normal file
373
cmvr-es/common/utils/visualization/image_display_test.cpp
Normal file
@ -0,0 +1,373 @@
|
||||
#include "common/utils/visualization/image_display.h"
|
||||
#include "common/utils/image/image_process.h"
|
||||
#include "devices/camera/realsense_camera/include/realsense_camera.h"
|
||||
#include "perception/include/tag_relative_target_3d.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <string>
|
||||
|
||||
namespace {
|
||||
|
||||
cv::Mat makeBlankImage(int width = 160, int height = 120) {
|
||||
return cv::Mat(height, width, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
}
|
||||
|
||||
constexpr const char* kRsSerial = "243122074587";
|
||||
constexpr double kTagSizeM = 0.01975;
|
||||
constexpr int kRsWidth = 1280;
|
||||
constexpr int kRsHeight = 720;
|
||||
constexpr int kRsFps = 30;
|
||||
constexpr int kTargetU = -1; // negative means image center
|
||||
constexpr int kTargetV = -1; // negative means image center
|
||||
constexpr int kDisplayFrames = 1200;
|
||||
constexpr const char* kTrackingWindowName = "ImageDisplayTagTrackingTest";
|
||||
|
||||
int countNonZeroPixelsInRoi(const cv::Mat& image, const cv::Rect& roi) {
|
||||
const cv::Rect image_rect(0, 0, image.cols, image.rows);
|
||||
const cv::Rect clipped = roi & image_rect;
|
||||
if (clipped.width <= 0 || clipped.height <= 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
cv::Mat gray;
|
||||
cv::cvtColor(image(clipped), gray, cv::COLOR_BGR2GRAY);
|
||||
return cv::countNonZero(gray);
|
||||
}
|
||||
|
||||
bool projectTargetToPixel(const Eigen::Vector3d& p_c,
|
||||
const cmvr::device::Rs2Intrinsics& intrinsics,
|
||||
cmvr::common::Pixel& pixel_out) {
|
||||
Eigen::Vector2d uv = Eigen::Vector2d::Zero();
|
||||
if (!cmvr::ImageProcess::projectCameraPointToPixel(intrinsics, p_c, uv)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
pixel_out.u = static_cast<int>(std::lround(uv.x()));
|
||||
pixel_out.v = static_cast<int>(std::lround(uv.y()));
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, RenderFailsWithoutImage) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
|
||||
cv::Mat rendered;
|
||||
EXPECT_FALSE(display.render(rendered));
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, RenderPointChangesCenterRegion) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
display.setImage(makeBlankImage());
|
||||
display.showPoint(cmvr::common::Pixel{60, 40},
|
||||
cmvr::common::Color::red(),
|
||||
5,
|
||||
-1,
|
||||
"target");
|
||||
|
||||
cv::Mat rendered;
|
||||
ASSERT_TRUE(display.render(rendered));
|
||||
ASSERT_FALSE(rendered.empty());
|
||||
|
||||
const cv::Vec3b center = rendered.at<cv::Vec3b>(40, 60);
|
||||
EXPECT_GT(static_cast<int>(center[2]), 150);
|
||||
EXPECT_LT(static_cast<int>(center[1]), 80);
|
||||
EXPECT_LT(static_cast<int>(center[0]), 80);
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, RenderCircleChangesExpectedArcRegion) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
display.setImage(makeBlankImage());
|
||||
display.showCircle(cmvr::common::Pixel{60, 40},
|
||||
12,
|
||||
cmvr::common::Color::yellow(),
|
||||
2);
|
||||
|
||||
cv::Mat rendered;
|
||||
ASSERT_TRUE(display.render(rendered));
|
||||
ASSERT_FALSE(rendered.empty());
|
||||
|
||||
EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(58, 26, 5, 5)), 0);
|
||||
EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 39, 3, 3)), 0);
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, RenderCrossChangesHorizontalAndVerticalArms) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
display.setImage(makeBlankImage());
|
||||
display.showCross(cmvr::common::Pixel{60, 40},
|
||||
10,
|
||||
cmvr::common::Color::blue(),
|
||||
2);
|
||||
|
||||
cv::Mat rendered;
|
||||
ASSERT_TRUE(display.render(rendered));
|
||||
ASSERT_FALSE(rendered.empty());
|
||||
|
||||
EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(49, 39, 5, 3)), 0);
|
||||
EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(59, 29, 3, 5)), 0);
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, RenderTextChangesRequestedRegion) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
display.setImage(makeBlankImage());
|
||||
|
||||
cmvr::common::TextOverlay overlay;
|
||||
overlay.text = "tracked tag: 7";
|
||||
overlay.position_px = cmvr::common::Pixel{10, 30};
|
||||
overlay.color = cmvr::common::Color::white();
|
||||
overlay.draw_background = true;
|
||||
|
||||
display.showText(overlay);
|
||||
|
||||
cv::Mat rendered;
|
||||
ASSERT_TRUE(display.render(rendered));
|
||||
ASSERT_FALSE(rendered.empty());
|
||||
|
||||
EXPECT_GT(countNonZeroPixelsInRoi(rendered, cv::Rect(5, 5, 120, 35)), 0);
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, ClearOverlaysRemovesAllRenderedGeometry) {
|
||||
cmvr::common::OpenCvImageCvDisplay display;
|
||||
display.setImage(makeBlankImage());
|
||||
display.showPoint(cmvr::common::Pixel{60, 40},
|
||||
cmvr::common::Color::red(),
|
||||
5,
|
||||
-1,
|
||||
"target");
|
||||
display.showCircle(cmvr::common::Pixel{60, 40},
|
||||
12,
|
||||
cmvr::common::Color::yellow(),
|
||||
2);
|
||||
display.showCross(cmvr::common::Pixel{60, 40},
|
||||
10,
|
||||
cmvr::common::Color::blue(),
|
||||
2);
|
||||
display.showText("tracked tag: 7",
|
||||
cmvr::common::Pixel{10, 30},
|
||||
cmvr::common::Color::white(),
|
||||
0.7,
|
||||
2);
|
||||
|
||||
display.clearOverlays();
|
||||
|
||||
cv::Mat rendered;
|
||||
ASSERT_TRUE(display.render(rendered));
|
||||
ASSERT_FALSE(rendered.empty());
|
||||
EXPECT_EQ(countNonZeroPixelsInRoi(rendered, cv::Rect(0, 0, rendered.cols, rendered.rows)), 0);
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, ShowDisplaysWindowIfHighGuiIsAvailable) {
|
||||
constexpr const char* kWindowName = "OpenCvImageCvDisplayTest";
|
||||
cmvr::common::OpenCvImageCvDisplay display(kWindowName);
|
||||
|
||||
bool window_enabled = true;
|
||||
try {
|
||||
cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
|
||||
cv::imshow(kWindowName, cv::Mat(120, 160, CV_8UC3, cv::Scalar(20, 20, 20)));
|
||||
cv::waitKey(1);
|
||||
} catch (const cv::Exception& e) {
|
||||
window_enabled = false;
|
||||
std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
|
||||
}
|
||||
|
||||
if (!window_enabled) {
|
||||
GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
|
||||
}
|
||||
|
||||
for (int frame = 0; frame < 1200; ++frame) {
|
||||
cv::Mat image(240, 320, CV_8UC3, cv::Scalar(20, 20, 20));
|
||||
display.setImage(image);
|
||||
display.clearOverlays();
|
||||
|
||||
const int center_u = 80 + frame;
|
||||
const int center_v = 120;
|
||||
|
||||
display.showPoint(cmvr::common::Pixel{center_u, center_v},
|
||||
cmvr::common::Color::red(),
|
||||
5,
|
||||
-1,
|
||||
"target");
|
||||
display.showCircle(cmvr::common::Pixel{center_u, center_v},
|
||||
18,
|
||||
cmvr::common::Color::yellow(),
|
||||
2);
|
||||
display.showCross(cmvr::common::Pixel{center_u, center_v},
|
||||
10,
|
||||
cmvr::common::Color::blue(),
|
||||
2);
|
||||
display.showText("tracked tag: 7",
|
||||
cmvr::common::Pixel{20, 30},
|
||||
cmvr::common::Color::white(),
|
||||
0.8,
|
||||
2);
|
||||
|
||||
const int key = display.show(30);
|
||||
if (key == 27 || key == 'q' || key == 'Q') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
display.close();
|
||||
}
|
||||
|
||||
TEST(OpenCvImageCvDisplayTest, ShowTrackingImageWithActiveTagAndTargetPoint) {
|
||||
const std::string serial = kRsSerial;
|
||||
if (serial.empty()) {
|
||||
GTEST_SKIP() << "kRsSerial is empty, please set it in image_display_test.cpp";
|
||||
}
|
||||
|
||||
cmvr::config::RealSenseCameraConfig cam_cfg;
|
||||
cam_cfg.set_id("image_display_test");
|
||||
cam_cfg.set_serialnumber(serial);
|
||||
cam_cfg.set_width(kRsWidth);
|
||||
cam_cfg.set_height(kRsHeight);
|
||||
cam_cfg.set_fps(kRsFps);
|
||||
cam_cfg.set_codec("H265");
|
||||
cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
|
||||
cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
|
||||
cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
|
||||
cam_cfg.set_buffer_size(30);
|
||||
cam_cfg.set_sync(true);
|
||||
cam_cfg.set_enable(true);
|
||||
|
||||
auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
|
||||
auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
|
||||
perception->setTagSize(kTagSizeM);
|
||||
|
||||
cmvr::perception::TagRelativeTarget3D tracker(perception);
|
||||
tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
|
||||
tracker.setActiveTagSwitchPolicy(4, 1.2);
|
||||
tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
|
||||
|
||||
cmvr::perception::AprilTagPerception::Options opt;
|
||||
opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
|
||||
opt.detect_tags = true;
|
||||
|
||||
ASSERT_NO_THROW(camera->init());
|
||||
ASSERT_NO_THROW(camera->start());
|
||||
struct CameraStopGuard {
|
||||
std::shared_ptr<cmvr::device::RealsenseCamera> cam;
|
||||
~CameraStopGuard() {
|
||||
if (!cam) return;
|
||||
try {
|
||||
cam->stop();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
} stop_guard{camera};
|
||||
|
||||
cmvr::common::OpenCvImageCvDisplay display(kTrackingWindowName);
|
||||
bool window_enabled = true;
|
||||
try {
|
||||
cv::namedWindow(kTrackingWindowName, cv::WINDOW_NORMAL);
|
||||
cv::resizeWindow(kTrackingWindowName, kRsWidth, kRsHeight);
|
||||
cv::imshow(kTrackingWindowName,
|
||||
cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20)));
|
||||
cv::waitKey(1);
|
||||
} catch (const cv::Exception& e) {
|
||||
window_enabled = false;
|
||||
std::cout << "[OpenCvImageCvDisplayTest] window disabled: " << e.what() << "\n";
|
||||
}
|
||||
if (!window_enabled) {
|
||||
GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
|
||||
}
|
||||
|
||||
bool target_uv_initialized = false;
|
||||
int target_u = kTargetU;
|
||||
int target_v = kTargetV;
|
||||
bool target_locked = false;
|
||||
|
||||
for (int frame = 0; frame < kDisplayFrames; ++frame) {
|
||||
const bool update_ok = perception->update(opt);
|
||||
const cv::Mat& color = perception->color();
|
||||
|
||||
cv::Mat display_image = color.empty()
|
||||
? cv::Mat(kRsHeight, kRsWidth, CV_8UC3, cv::Scalar(20, 20, 20))
|
||||
: color;
|
||||
display.setImage(display_image);
|
||||
display.clearOverlays();
|
||||
|
||||
if (!target_uv_initialized && !display_image.empty()) {
|
||||
target_u = (kTargetU >= 0) ? kTargetU : (display_image.cols / 2);
|
||||
target_v = (kTargetV >= 0) ? kTargetV : (display_image.rows / 2);
|
||||
target_uv_initialized = true;
|
||||
}
|
||||
|
||||
if (target_uv_initialized) {
|
||||
const cmvr::common::Pixel selected_px{target_u, target_v};
|
||||
display.showCross(selected_px, 10, cmvr::common::Color::yellow(), 2);
|
||||
display.showCircle(selected_px, 14, cmvr::common::Color::yellow(), 2);
|
||||
}
|
||||
|
||||
bool tracker_ok = false;
|
||||
if (target_uv_initialized && !color.empty()) {
|
||||
if (!target_locked) {
|
||||
tracker_ok = tracker.startTrackingFromPixel(target_u, target_v);
|
||||
target_locked = tracker_ok;
|
||||
} else {
|
||||
tracker_ok = tracker.track();
|
||||
}
|
||||
}
|
||||
|
||||
if (tracker_ok) {
|
||||
cmvr::common::Pixel target_px{};
|
||||
if (projectTargetToPixel(tracker.lastTargetInCamera(), perception->intrinsics(), target_px)) {
|
||||
display.showPoint(target_px,
|
||||
cmvr::common::Color::red(),
|
||||
5,
|
||||
-1,
|
||||
"target");
|
||||
display.showCircle(target_px, 18, cmvr::common::Color::red(), 2);
|
||||
}
|
||||
}
|
||||
|
||||
display.showText("active tag: " + std::to_string(tracker.activeTagId()),
|
||||
cmvr::common::Pixel{20, 30},
|
||||
cmvr::common::Color::white(),
|
||||
0.8,
|
||||
2);
|
||||
display.showText("perception: " +
|
||||
std::string(cmvr::perception::AprilTagPerception::statusToString(
|
||||
perception->lastStatus())),
|
||||
cmvr::common::Pixel{20, 60},
|
||||
cmvr::common::Color::white(),
|
||||
0.7,
|
||||
2);
|
||||
display.showText("tracker: " +
|
||||
std::string(cmvr::perception::TagRelativeTarget3D::statusToString(
|
||||
tracker.lastStatus())),
|
||||
cmvr::common::Pixel{20, 90},
|
||||
cmvr::common::Color::white(),
|
||||
0.7,
|
||||
2);
|
||||
display.showText("tags: " + std::to_string(perception->tags().size()) +
|
||||
" frame_ok: " + std::string(update_ok ? "true" : "false"),
|
||||
cmvr::common::Pixel{20, 120},
|
||||
cmvr::common::Color::white(),
|
||||
0.7,
|
||||
2);
|
||||
|
||||
if (tracker_ok) {
|
||||
const Eigen::Vector3d& p_c_target = tracker.lastTargetInCamera();
|
||||
display.showText("target_c: [" +
|
||||
std::to_string(p_c_target.x()) + ", " +
|
||||
std::to_string(p_c_target.y()) + ", " +
|
||||
std::to_string(p_c_target.z()) + "]",
|
||||
cmvr::common::Pixel{20, 150},
|
||||
cmvr::common::Color::white(),
|
||||
0.6,
|
||||
2);
|
||||
}
|
||||
|
||||
const int key = display.show(1);
|
||||
if (key == 27 || key == 'q' || key == 'Q') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
display.close();
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
//
|
||||
// Created by lgv on 2026/2/26.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
#ifndef CMVR_IBVS_CONTROLLER_H
|
||||
#define CMVR_IBVS_CONTROLLER_H
|
||||
|
||||
#include <array>
|
||||
#include <memory>
|
||||
@ -13,52 +11,53 @@
|
||||
|
||||
#include <visp3/core/vpHomogeneousMatrix.h>
|
||||
#include <visp3/core/vpPoint.h>
|
||||
#include <visp3/detection/vpDetectorAprilTag.h>
|
||||
#include <visp3/visual_features/vpFeaturePoint.h>
|
||||
#include <visp3/vs/vpServo.h>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
||||
#include "perception/include/apriltag_perception.h"
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
/**
|
||||
* @brief 基于 AprilTag + ViSP + Pinocchio 的眼在手上 IBVS 控制器。
|
||||
* @brief 基于 AprilTag 观测的眼在手上 IBVS 控制器。
|
||||
*
|
||||
* 坐标系约定：
|
||||
* - `t`：当前被跟踪的 tag 坐标系。
|
||||
* - `c`：ViSP 相机坐标系，也是视觉伺服任务中使用的相机坐标系。
|
||||
* - `cam`：`AbstractCamera` 定义的相机坐标系。
|
||||
* - `u`：URDF / IK 求解器使用的相机坐标系。
|
||||
*
|
||||
* 主要数据流：
|
||||
* - `AprilTagPerception` 提供 tag 位姿 `cMo` / `T_c_t`；
|
||||
* - 本类在相机坐标系 `c` 中构造视觉伺服误差；
|
||||
* - 再将相机 twist 从 `c` 转到 `cam` 和 `u`，最后交给速度 IK。
|
||||
*/
|
||||
class IbvsController {
|
||||
public:
|
||||
/**
|
||||
* @brief 深度使用模式。
|
||||
*/
|
||||
enum class DepthMode {
|
||||
MONOCULAR = 0, /**< 仅使用 AprilTag 位姿估计得到的深度。 */
|
||||
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<device::AbstractCamera>& 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<cmvr::perception::AprilTagPerception>& perception);
|
||||
|
||||
const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception() const { return perception_; }
|
||||
|
||||
/**
|
||||
* @brief 重置内部状态与关节命令缓存。
|
||||
* @param q_init 初始关节命令；为空时清空缓存。
|
||||
* @param q_init 初始关节位置命令；为空时清空内部缓存。
|
||||
*/
|
||||
void reset(const std::vector<double>& q_init = {});
|
||||
|
||||
/**
|
||||
* @brief 根据当前图像与关节角计算下一拍关节位置命令。
|
||||
* @brief 根据当前关节状态计算下一拍关节位置命令。
|
||||
* @param joints_angle 当前关节角。
|
||||
* @param dt 控制周期（秒）。
|
||||
* @param dt 控制周期，单位秒。
|
||||
* @param q_cmd_out 输出的下一拍关节位置命令。
|
||||
* @return 计算成功返回 `true`。
|
||||
*/
|
||||
bool compute(const std::vector<double>& joints_angle,
|
||||
double dt,
|
||||
std::vector<double>& q_cmd_out);
|
||||
|
||||
/**
|
||||
* @brief 根据当前图像与关节角计算目标关节速度命令。
|
||||
* @brief 根据当前关节状态计算关节速度命令。
|
||||
* @param joints_angle 当前关节角。
|
||||
* @param qdot_out 输出的关节速度命令（rad/s）。
|
||||
* @param qdot_out 输出的关节速度命令。
|
||||
* @return 计算成功返回 `true`。
|
||||
*/
|
||||
bool compute(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out);
|
||||
|
||||
/**
|
||||
* @brief 设置 ViSP 控制增益。
|
||||
* @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<double, 6>& 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<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 核心计算链路：图像 -> 相机 twist -> 关节速度。
|
||||
* @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。
|
||||
* @param joints_angle 当前关节角。
|
||||
* @param qdot_out 输出的关节速度命令（rad/s）。
|
||||
* @param qdot_out 输出的关节速度命令。
|
||||
* @return 计算成功返回 `true`。
|
||||
*/
|
||||
bool computeInternal(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out);
|
||||
|
||||
/**
|
||||
* @brief 按 URDF 关节上下限对关节命令做硬裁剪。
|
||||
* @param q 关节命令，函数内原地修改。
|
||||
* @brief 按 URDF 关节上下限对关节命令做原地裁剪。
|
||||
* @param q 关节命令向量。
|
||||
*/
|
||||
void clampJointCommandInPlace(std::vector<double>& q) const;
|
||||
|
||||
/**
|
||||
* @brief 根据当前目标位姿反解 tag 平面的深度控制点。
|
||||
* @brief 从共享感知前端同步 tag 尺寸到控制模型。
|
||||
* @param force 为 `true` 时即使尺寸未变化也强制重建任务。
|
||||
*/
|
||||
void syncTagSizeFromPerception(bool force = false);
|
||||
|
||||
/**
|
||||
* @brief 根据当前期望位姿 `cMo_des`，反算深度闭环控制点在 tag 坐标系 `t` 中的坐标。
|
||||
*/
|
||||
void updateDepthControlPointInTag();
|
||||
|
||||
/**
|
||||
* @brief 重新构建 ViSP 任务与期望特征。
|
||||
* @brief 按当前期望位姿和 tag 几何重新构建 ViSP 视觉伺服任务。
|
||||
*/
|
||||
void initTask();
|
||||
|
||||
private:
|
||||
// 是否初始化成功。
|
||||
// 控制器和 IK 求解器是否已成功初始化。
|
||||
bool initialized_{false};
|
||||
// 速度 IK 使用的基座 frame 名称。
|
||||
std::string base_frame_name_;
|
||||
// 速度 IK 使用的末端相机 frame 名称。
|
||||
std::string camera_frame_name_;
|
||||
// 相机对象。
|
||||
std::shared_ptr<device::AbstractCamera> camera_{nullptr};
|
||||
|
||||
// 参数
|
||||
// ViSP 控制增益。
|
||||
// IK 链基座 frame 名称。
|
||||
std::string base_frame_name_;
|
||||
|
||||
// URDF 中相机 frame 名称，对应坐标系 `u`。
|
||||
std::string camera_frame_name_;
|
||||
|
||||
// 共享感知前端。
|
||||
std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
|
||||
|
||||
// ViSP 视觉伺服增益。
|
||||
double lambda_{0.7};
|
||||
// tag 边长（米）。
|
||||
|
||||
// 控制模型使用的 tag 边长，单位米。
|
||||
double tag_size_m_{0.12};
|
||||
// tag 半边长（米）。
|
||||
|
||||
// 控制模型使用的 tag 半边长，单位米。
|
||||
double tag_half_{0.06};
|
||||
// 期望平移 x（米）。
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。
|
||||
double target_x_{0.0};
|
||||
// 期望平移 y（米）。
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。
|
||||
double target_y_{0.0};
|
||||
// 期望平移 z（米）。
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。
|
||||
double target_z_{0.33};
|
||||
// 期望旋转参数 rx（弧度）。
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度。
|
||||
double target_rx_{3.14159265358979323846};
|
||||
// 期望旋转参数 ry（弧度）。
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度。
|
||||
double target_ry_{0.0};
|
||||
// 期望旋转参数 rz（弧度）。
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 rz，单位弧度。
|
||||
double target_rz_{0.0};
|
||||
// DLS 阻尼系数。
|
||||
|
||||
// DLS IK 阻尼系数。
|
||||
double mu_{0.02};
|
||||
// 关节速度绝对值上限（rad/s）。
|
||||
|
||||
// 关节速度上限。
|
||||
double qdot_max_{0.6};
|
||||
// 深度模式。
|
||||
|
||||
// 深度使用模式。
|
||||
DepthMode depth_mode_{DepthMode::MONOCULAR};
|
||||
// 深度闭环增益：`vz = kp * (z_cur - target_z_)`。
|
||||
|
||||
// 深度闭环比例增益。
|
||||
double depth_z_kp_{1.0};
|
||||
|
||||
// 相机 twist 六维限幅。
|
||||
std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
|
||||
// 关节限位避障 null-space 参数。
|
||||
|
||||
// 是否启用关节限位回避。
|
||||
bool limit_avoidance_enabled_{false};
|
||||
|
||||
// 关节限位回避增益。
|
||||
double limit_avoidance_gain_{0.2};
|
||||
|
||||
// 关节限位回避触发边界比例。
|
||||
double limit_avoidance_margin_ratio_{0.15};
|
||||
|
||||
// 单关节最大推回速度。
|
||||
double limit_avoidance_max_push_{0.25};
|
||||
// tag 平面中用于采样深度的控制点。
|
||||
|
||||
// 用于深度闭环的 tag 平面控制点，位于 tag 坐标系 `t` 的 xy 平面，单位米。
|
||||
Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
|
||||
|
||||
// 坐标对齐
|
||||
// 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<vpServo> task_{nullptr};
|
||||
// tag 四角点（3D）。
|
||||
|
||||
// tag 四个角点在 tag 坐标系 `t` 中的 3D 模型点。
|
||||
vpPoint obj_pts_[4];
|
||||
// 当前特征。
|
||||
|
||||
// 当前观测特征。
|
||||
vpFeaturePoint s_cur_[4];
|
||||
// 目标特征。
|
||||
|
||||
// 期望特征。
|
||||
vpFeaturePoint s_star_[4];
|
||||
// AprilTag 检测器。
|
||||
vpDetectorAprilTag detector_;
|
||||
// 指定跟踪的 tag id（必填）；<0 表示未设置。
|
||||
|
||||
// 当前配置要跟踪的 tag id。
|
||||
int tracked_tag_id_{-1};
|
||||
|
||||
// 速度 IK 求解器。
|
||||
std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
|
||||
// 是否成功读取到 URDF 关节限位。
|
||||
|
||||
// 是否成功读取 URDF 中的关节位置限位。
|
||||
bool has_joint_position_limits_{false};
|
||||
// 本链关节位置下限（rad）。
|
||||
|
||||
// 关节位置下限。
|
||||
Eigen::VectorXd q_lower_limits_;
|
||||
// 本链关节位置上限（rad）。
|
||||
|
||||
// 关节位置上限。
|
||||
Eigen::VectorXd q_upper_limits_;
|
||||
// 最近一次用于控制的 tag id。
|
||||
|
||||
// 最近一次成功控制时实际使用的 tag id。
|
||||
int last_used_tag_id_{-1};
|
||||
|
||||
// 内部积分得到的关节位置命令缓存。
|
||||
std::vector<double> q_cmd_;
|
||||
// 最近一帧 tag 检测结果。
|
||||
|
||||
// 最近一帧是否检测到了当前被跟踪的 tag。
|
||||
bool last_tag_detected_{false};
|
||||
// 最近一次 `compute()` 状态。
|
||||
|
||||
// 最近一次 `compute()` 的状态。
|
||||
ComputeStatus last_compute_status_{ComputeStatus::NOT_READY};
|
||||
// 最近一帧深度来源。
|
||||
|
||||
// 最近一次 `compute()` 实际使用的深度来源。
|
||||
DepthUsage last_depth_usage_{DepthUsage::NONE};
|
||||
// 最近一帧 tag 平移（ViSP 相机系）。
|
||||
|
||||
// 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
|
||||
Eigen::Vector3d last_tag_pos_visp_{Eigen::Vector3d::Zero()};
|
||||
// 最近一帧相机 twist（ViSP 相机系）。
|
||||
|
||||
// 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
|
||||
Eigen::Matrix<double, 6, 1> last_v_camera_visp_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
#endif // CMVR_IBVS_CONTROLLER_H
|
||||
|
||||
@ -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<cmvr::perception::AprilTagPerception>(mujoco_camera_);
|
||||
perception_->setTagSize(tag_size_m_);
|
||||
perception_opt_.detect_tags = true;
|
||||
perception_opt_.fetch_encoded = false;
|
||||
perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
|
||||
ibvs_controller_->setPerception(perception_);
|
||||
|
||||
std::vector<double> q_init(7, 0.0);
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
q_init[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
|
||||
@ -609,6 +617,26 @@ protected:
|
||||
q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
|
||||
}
|
||||
|
||||
if (perception_) {
|
||||
switch (ibvs_controller_->depthMode()) {
|
||||
case IbvsController::DepthMode::MONOCULAR:
|
||||
perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
|
||||
break;
|
||||
case IbvsController::DepthMode::PREFER_DEPTH:
|
||||
perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER;
|
||||
break;
|
||||
case IbvsController::DepthMode::DEPTH_ONLY:
|
||||
perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
|
||||
break;
|
||||
}
|
||||
const bool perception_ok = perception_->update(perception_opt_);
|
||||
if (!perception_ok && (step_count_ % 60) == 0) {
|
||||
std::cout << "[IBVS] perception update failed: "
|
||||
<< cmvr::perception::AprilTagPerception::statusToString(perception_->lastStatus())
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<double> q_cmd_next;
|
||||
const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next);
|
||||
|
||||
@ -617,6 +645,13 @@ protected:
|
||||
const auto st = ibvs_controller_->lastComputeStatus();
|
||||
std::cout << "[IBVS] compute skipped: "
|
||||
<< IbvsController::statusToString(st) << std::endl;
|
||||
if (st == IbvsController::ComputeStatus::TAG_MISMATCH && perception_ && perception_->hasTags()) {
|
||||
std::cout << "[IBVS] visible tag ids:";
|
||||
for (const auto& t : perception_->tags()) {
|
||||
std::cout << " " << t.id;
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
}
|
||||
++step_count_;
|
||||
return;
|
||||
@ -698,9 +733,12 @@ private:
|
||||
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
|
||||
std::string camera_frame_name_for_check_{"R_CAM"};
|
||||
int tracked_tag_id_{0};
|
||||
double tag_size_m_{0.12};
|
||||
|
||||
std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
|
||||
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
|
||||
std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
|
||||
cmvr::perception::AprilTagPerception::Options perception_opt_{};
|
||||
|
||||
std::array<double, 7> q_cmd_{{0, 0, 0, 0, 0, 0, 0}};
|
||||
int step_count_{0};
|
||||
|
||||
@ -1,7 +1,3 @@
|
||||
//
|
||||
// Created by lgv on 2026/2/26.
|
||||
//
|
||||
|
||||
#include "controller/include/ibvs_controller.h"
|
||||
|
||||
#include "common/utils/image/image_process.h"
|
||||
@ -13,14 +9,11 @@
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <visp3/core/vpCameraParameters.h>
|
||||
#include <visp3/core/vpImage.h>
|
||||
#include <visp3/core/vpRotationMatrix.h>
|
||||
#include <visp3/core/vpTranslationVector.h>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
|
||||
|
||||
const char* IbvsController::statusToString(ComputeStatus status) {
|
||||
switch (status) {
|
||||
case ComputeStatus::OK: return "ok";
|
||||
@ -46,39 +39,21 @@ const char* IbvsController::depthUsageToString(DepthUsage usage) {
|
||||
}
|
||||
}
|
||||
|
||||
IbvsController::IbvsController()
|
||||
: 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<device::AbstractCamera>& 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<PinocchioDlsIKSolver>(
|
||||
urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_);
|
||||
|
||||
@ -95,10 +70,35 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
q_lower_limits_.resize(0);
|
||||
q_upper_limits_.resize(0);
|
||||
}
|
||||
|
||||
last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
void IbvsController::setPerception(const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception) {
|
||||
perception_ = perception;
|
||||
// 注入后立刻同步一次 tag size（用于控制模型）
|
||||
syncTagSizeFromPerception(true);
|
||||
}
|
||||
|
||||
void IbvsController::syncTagSizeFromPerception(bool force) {
|
||||
if (!perception_) return;
|
||||
|
||||
const double s = perception_->tagSize();
|
||||
if (!std::isfinite(s) || s <= 0.0) return;
|
||||
|
||||
if (!force && std::abs(s - tag_size_m_) < 1e-12) return;
|
||||
|
||||
tag_size_m_ = s;
|
||||
tag_half_ = 0.5 * tag_size_m_;
|
||||
|
||||
// 深度控制点约束依赖 tag_half_
|
||||
updateDepthControlPointInTag();
|
||||
|
||||
// 任务几何依赖 tag_half_
|
||||
initTask();
|
||||
}
|
||||
|
||||
void IbvsController::reset(const std::vector<double>& q_init) {
|
||||
if (q_init.empty()) {
|
||||
q_cmd_.clear();
|
||||
@ -106,6 +106,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
|
||||
q_cmd_ = q_init;
|
||||
clampJointCommandInPlace(q_cmd_);
|
||||
}
|
||||
|
||||
last_tag_detected_ = false;
|
||||
last_used_tag_id_ = -1;
|
||||
last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
|
||||
@ -114,215 +115,6 @@ void IbvsController::reset(const std::vector<double>& q_init) {
|
||||
last_v_camera_visp_.setZero();
|
||||
}
|
||||
|
||||
bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out) {
|
||||
last_depth_usage_ = DepthUsage::NONE;
|
||||
|
||||
if (!initialized_ || !camera_ || !dls_solver_) {
|
||||
last_compute_status_ = ComputeStatus::NOT_READY;
|
||||
return false;
|
||||
}
|
||||
if (joints_angle.empty()) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
|
||||
cv::Mat color;
|
||||
cv::Mat depth;
|
||||
device::Rs2Intrinsics intrinsics{};
|
||||
camera_->getRGBDImages(color, depth, intrinsics);
|
||||
|
||||
if (color.empty()) {
|
||||
last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
if (color.channels() != 3) {
|
||||
if (color.channels() != 1 && color.channels() != 4) {
|
||||
last_compute_status_ = ComputeStatus::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const double fx = static_cast<double>(intrinsics.fx);
|
||||
const double fy = static_cast<double>(intrinsics.fy);
|
||||
const double cx = static_cast<double>(intrinsics.cx);
|
||||
const double cy = static_cast<double>(intrinsics.cy);
|
||||
if (fx <= 0.0 || fy <= 0.0) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
|
||||
cv::Mat gray;
|
||||
if (color.channels() == 1) {
|
||||
gray = color;
|
||||
} else if (color.channels() == 3) {
|
||||
cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
|
||||
} else {
|
||||
cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
|
||||
}
|
||||
if (gray.empty() || gray.type() != CV_8UC1) {
|
||||
last_compute_status_ = ComputeStatus::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
if (!gray.isContinuous()) {
|
||||
gray = gray.clone();
|
||||
}
|
||||
|
||||
const int width = gray.cols;
|
||||
const int height = gray.rows;
|
||||
vpCameraParameters cam;
|
||||
cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
|
||||
|
||||
vpImage<unsigned char> I(height, width);
|
||||
for (int y = 0; y < height; ++y) {
|
||||
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
|
||||
}
|
||||
|
||||
if (tracked_tag_id_ < 0) {
|
||||
last_tag_detected_ = false;
|
||||
last_used_tag_id_ = -1;
|
||||
last_tag_pos_visp_.setZero();
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<vpHomogeneousMatrix> cMo_vec;
|
||||
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
|
||||
if (!detected || cMo_vec.empty()) {
|
||||
last_tag_detected_ = false;
|
||||
last_used_tag_id_ = -1;
|
||||
last_tag_pos_visp_.setZero();
|
||||
last_compute_status_ = ComputeStatus::NO_TAG;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 只允许使用指定 id 的 tag，不再回退到“第一个检测结果”。
|
||||
const std::vector<int> tag_ids = detector_.getTagsId();
|
||||
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
|
||||
int selected_idx = -1;
|
||||
for (size_t i = 0; i < pair_size; ++i) {
|
||||
if (tag_ids[i] == tracked_tag_id_) {
|
||||
selected_idx = static_cast<int>(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (selected_idx < 0) {
|
||||
std::cout << "[IbvsController] TAG_MISMATCH target_tag_id=" << tracked_tag_id_
|
||||
<< ", detected_tag_ids=[";
|
||||
for (size_t i = 0; i < tag_ids.size(); ++i) {
|
||||
if (i > 0) std::cout << ",";
|
||||
std::cout << tag_ids[i];
|
||||
}
|
||||
std::cout << "]\n";
|
||||
|
||||
last_tag_detected_ = false;
|
||||
last_used_tag_id_ = -1;
|
||||
last_tag_pos_visp_.setZero();
|
||||
last_compute_status_ = ComputeStatus::TAG_MISMATCH;
|
||||
return false;
|
||||
}
|
||||
|
||||
last_tag_detected_ = true;
|
||||
last_used_tag_id_ = tracked_tag_id_;
|
||||
vpHomogeneousMatrix cMo = cMo_vec[static_cast<size_t>(selected_idx)];
|
||||
last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
|
||||
|
||||
// 深度控制点定义在 tag 平面（object frame）:
|
||||
// p_o = [x_t, y_t, 0, 1]^T
|
||||
// 经位姿变换后在相机系:
|
||||
// p_c = cMo * p_o = [X, Y, Z, 1]^T
|
||||
// vpPoint::get_x/get_y 给的是归一化坐标 x=X/Z, y=Y/Z。
|
||||
vpPoint depth_ctrl_pt;
|
||||
depth_ctrl_pt.setWorldCoordinates(depth_control_point_tag_.x(), depth_control_point_tag_.y(), 0.0);
|
||||
depth_ctrl_pt.track(cMo);
|
||||
|
||||
const double x_depth_ctrl = depth_ctrl_pt.get_x();
|
||||
const double y_depth_ctrl = depth_ctrl_pt.get_y();
|
||||
double z_depth_ctrl = std::max(depth_ctrl_pt.get_Z(), 0.05);
|
||||
bool depth_ctrl_used = false;
|
||||
|
||||
if (depth_mode_ != DepthMode::MONOCULAR) {
|
||||
// 针孔投影:
|
||||
// u = fx * x + cx
|
||||
// v = fy * y + cy
|
||||
const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
|
||||
const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
|
||||
double z_from_depth = 0.0;
|
||||
if (ImageProcess::sampleDepthMeters(depth, u, v, z_from_depth)) {
|
||||
z_depth_ctrl = std::max(z_from_depth, 0.05);
|
||||
depth_ctrl_used = true;
|
||||
} else if (depth_mode_ == DepthMode::DEPTH_ONLY) {
|
||||
last_compute_status_ = ComputeStatus::NO_DEPTH;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
obj_pts_[i].track(cMo);
|
||||
const double x = obj_pts_[i].get_x();
|
||||
const double y = obj_pts_[i].get_y();
|
||||
const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
|
||||
s_cur_[i].buildFrom(x, y, Z);
|
||||
}
|
||||
|
||||
if (depth_ctrl_used) {
|
||||
last_depth_usage_ = DepthUsage::DEPTH_ONLY;
|
||||
} else {
|
||||
last_depth_usage_ = DepthUsage::POSE_ONLY;
|
||||
}
|
||||
|
||||
vpColVector v_c = task_->computeControlLaw();
|
||||
|
||||
// 深度闭环（仅替换 z 方向）:
|
||||
// e_z = z_cur - z_target
|
||||
// v_z = k_p * e_z
|
||||
// 其余 5 维仍沿用 ViSP IBVS 控制律输出。
|
||||
if (depth_ctrl_used) {
|
||||
v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
|
||||
last_v_camera_visp_[i] = v_c[i];
|
||||
}
|
||||
|
||||
Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
|
||||
Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
|
||||
// R_cv_: AbstractCamera -> ViSP，所以从 ViSP 回到 AbstractCamera 要乘转置:
|
||||
// v_cam = R_cv^T * v_visp
|
||||
// w_cam = R_cv^T * w_visp
|
||||
Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
|
||||
Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
|
||||
|
||||
Eigen::Matrix<double, 6, 1> twist_ee;
|
||||
twist_ee << v_site(0), v_site(1), v_site(2), w_site(0), w_site(1), w_site(2);
|
||||
|
||||
Eigen::Matrix<double, 6, 1> twist_ee_pin;
|
||||
// 线速度和角速度都用同一旋转做坐标变换:
|
||||
// v_urdf = R_camera_urdf * v_cam
|
||||
// w_urdf = R_camera_urdf * w_cam
|
||||
twist_ee_pin.head<3>() = R_camera_urdf_ * twist_ee.head<3>();
|
||||
twist_ee_pin.tail<3>() = R_camera_urdf_ * twist_ee.tail<3>();
|
||||
|
||||
dls_solver_->update_joints_state(joints_angle);
|
||||
std::vector<double> qdot;
|
||||
const bool ok = dls_solver_->ik(
|
||||
base_frame_name_, camera_frame_name_, twist_ee_pin,
|
||||
qdot, mu_, std::numeric_limits<double>::infinity());
|
||||
if (!ok || qdot.size() != joints_angle.size()) {
|
||||
last_compute_status_ = ComputeStatus::IK_FAILED;
|
||||
return false;
|
||||
}
|
||||
|
||||
qdot_out.resize(qdot.size());
|
||||
for (size_t i = 0; i < qdot.size(); ++i) {
|
||||
qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
|
||||
}
|
||||
|
||||
last_compute_status_ = ComputeStatus::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
double dt,
|
||||
std::vector<double>& q_cmd_out) {
|
||||
@ -341,19 +133,10 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < qdot.size(); ++i) {
|
||||
// 显式欧拉积分:
|
||||
// q_{k+1} = q_k + qdot * dt
|
||||
q_cmd_[i] += qdot[i] * dt;
|
||||
}
|
||||
|
||||
|
||||
// std::cout << "b_cmd: " ;
|
||||
// for (const auto& it : q_cmd_) {
|
||||
// std::cout << it << " ";
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
clampJointCommandInPlace(q_cmd_);
|
||||
|
||||
q_cmd_out = q_cmd_;
|
||||
return true;
|
||||
}
|
||||
@ -363,6 +146,153 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
return computeInternal(joints_angle, qdot_out);
|
||||
}
|
||||
|
||||
bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out) {
|
||||
last_depth_usage_ = DepthUsage::NONE;
|
||||
last_tag_detected_ = false;
|
||||
last_used_tag_id_ = -1;
|
||||
last_tag_pos_visp_.setZero();
|
||||
last_v_camera_visp_.setZero();
|
||||
|
||||
if (!initialized_ || !dls_solver_) {
|
||||
last_compute_status_ = ComputeStatus::NOT_READY;
|
||||
return false;
|
||||
}
|
||||
if (!perception_) {
|
||||
last_compute_status_ = ComputeStatus::NOT_READY;
|
||||
return false;
|
||||
}
|
||||
if (joints_angle.empty()) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
if (tracked_tag_id_ < 0) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 每帧确保 tag_size 同步（你可能运行时调 perception->setTagSize()）
|
||||
syncTagSizeFromPerception(false);
|
||||
|
||||
// perception 必须先 update()，这里以 color 是否为空作为“是否ready”的简单判据
|
||||
if (perception_->color().empty()) {
|
||||
last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!perception_->hasTags()) {
|
||||
last_compute_status_ = ComputeStatus::NO_TAG;
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto* tag = perception_->findTag(tracked_tag_id_);
|
||||
if (!tag) {
|
||||
last_compute_status_ = ComputeStatus::TAG_MISMATCH;
|
||||
return false;
|
||||
}
|
||||
|
||||
last_tag_detected_ = true;
|
||||
last_used_tag_id_ = tracked_tag_id_;
|
||||
|
||||
const vpHomogeneousMatrix& cMo = tag->cMo;
|
||||
last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
|
||||
|
||||
// intrinsics
|
||||
const auto& intr = perception_->intrinsics();
|
||||
const double fx = static_cast<double>(intr.fx);
|
||||
const double fy = static_cast<double>(intr.fy);
|
||||
const double cx = static_cast<double>(intr.cx);
|
||||
const double cy = static_cast<double>(intr.cy);
|
||||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 深度控制点（先用位姿 Z）
|
||||
vpPoint depth_ctrl_pt;
|
||||
depth_ctrl_pt.setWorldCoordinates(depth_control_point_tag_.x(), depth_control_point_tag_.y(), 0.0);
|
||||
depth_ctrl_pt.track(cMo);
|
||||
|
||||
const double x_depth_ctrl = depth_ctrl_pt.get_x();
|
||||
const double y_depth_ctrl = depth_ctrl_pt.get_y();
|
||||
double z_depth_ctrl = std::max(depth_ctrl_pt.get_Z(), 0.05);
|
||||
bool depth_ctrl_used = false;
|
||||
|
||||
const cv::Mat& depth = perception_->depth();
|
||||
if (depth_mode_ != DepthMode::MONOCULAR) {
|
||||
const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
|
||||
const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
|
||||
|
||||
double z_from_depth = 0.0;
|
||||
if (!depth.empty() && ImageProcess::sampleDepthMeters(depth, u, v, z_from_depth)) {
|
||||
z_depth_ctrl = std::max(z_from_depth, 0.05);
|
||||
depth_ctrl_used = true;
|
||||
} else if (depth_mode_ == DepthMode::DEPTH_ONLY) {
|
||||
last_compute_status_ = ComputeStatus::NO_DEPTH;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 当前特征（四角点用 pose Z）
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
obj_pts_[i].track(cMo);
|
||||
const double x = obj_pts_[i].get_x();
|
||||
const double y = obj_pts_[i].get_y();
|
||||
const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
|
||||
s_cur_[i].buildFrom(x, y, Z);
|
||||
}
|
||||
|
||||
last_depth_usage_ = depth_ctrl_used ? DepthUsage::DEPTH_ONLY : DepthUsage::POSE_ONLY;
|
||||
|
||||
vpColVector v_c = task_->computeControlLaw();
|
||||
|
||||
// 深度闭环：仅替换 vz
|
||||
if (depth_ctrl_used) {
|
||||
v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
|
||||
}
|
||||
|
||||
// 限幅+缓存（ViSP camera系）
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
|
||||
last_v_camera_visp_[i] = v_c[i];
|
||||
}
|
||||
|
||||
// ViSP -> AbstractCamera
|
||||
Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
|
||||
Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
|
||||
Eigen::Vector3d v_cam = R_cv_.transpose() * v_visp;
|
||||
Eigen::Vector3d w_cam = R_cv_.transpose() * w_visp;
|
||||
|
||||
Eigen::Matrix<double, 6, 1> twist_cam;
|
||||
twist_cam << v_cam(0), v_cam(1), v_cam(2), w_cam(0), w_cam(1), w_cam(2);
|
||||
|
||||
// AbstractCamera -> URDF camera
|
||||
Eigen::Matrix<double, 6, 1> twist_urdf;
|
||||
twist_urdf.head<3>() = R_camera_urdf_ * twist_cam.head<3>();
|
||||
twist_urdf.tail<3>() = R_camera_urdf_ * twist_cam.tail<3>();
|
||||
|
||||
// IK
|
||||
dls_solver_->update_joints_state(joints_angle);
|
||||
|
||||
std::vector<double> qdot;
|
||||
const bool ok = dls_solver_->ik(
|
||||
base_frame_name_, camera_frame_name_, twist_urdf,
|
||||
qdot, mu_, std::numeric_limits<double>::infinity());
|
||||
|
||||
if (!ok || qdot.size() != joints_angle.size()) {
|
||||
last_compute_status_ = ComputeStatus::IK_FAILED;
|
||||
return false;
|
||||
}
|
||||
|
||||
qdot_out.resize(qdot.size());
|
||||
for (size_t i = 0; i < qdot.size(); ++i) {
|
||||
qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
|
||||
}
|
||||
|
||||
last_compute_status_ = ComputeStatus::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
void IbvsController::clampJointCommandInPlace(std::vector<double>& q) const {
|
||||
if (!has_joint_position_limits_) return;
|
||||
if (q.size() != static_cast<size_t>(q_lower_limits_.size()) ||
|
||||
@ -382,12 +312,6 @@ void IbvsController::setLambda(double lambda) {
|
||||
initTask();
|
||||
}
|
||||
|
||||
void IbvsController::setTagSize(double tag_size_m) {
|
||||
tag_size_m_ = tag_size_m;
|
||||
tag_half_ = tag_size_m_ * 0.5;
|
||||
initTask();
|
||||
}
|
||||
|
||||
void IbvsController::setTrackedTagId(int tag_id) {
|
||||
tracked_tag_id_ = tag_id;
|
||||
}
|
||||
@ -408,6 +332,39 @@ void IbvsController::setTarget(double x,
|
||||
initTask();
|
||||
}
|
||||
|
||||
bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
|
||||
const Eigen::Vector3d& p_c_target_des,
|
||||
double rx,
|
||||
double ry,
|
||||
double rz) {
|
||||
if (!p_t_target.allFinite() || !p_c_target_des.allFinite()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
vpRotationMatrix R_des_visp;
|
||||
R_des_visp.buildFrom(rx, ry, rz); // 注意：这是 rotvec（theta*u）
|
||||
|
||||
Eigen::Matrix3d R_des;
|
||||
for (int r = 0; r < 3; ++r) {
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
R_des(r, c) = R_des_visp[r][c];
|
||||
}
|
||||
}
|
||||
|
||||
if (!R_des.allFinite()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const Eigen::Vector3d t_des = p_c_target_des - R_des * p_t_target;
|
||||
|
||||
if (!t_des.allFinite()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
setTarget(t_des.x(), t_des.y(), t_des.z(), rx, ry, rz);
|
||||
return true;
|
||||
}
|
||||
|
||||
void IbvsController::setMu(double mu) {
|
||||
mu_ = mu;
|
||||
}
|
||||
@ -436,6 +393,7 @@ void IbvsController::setJointLimitAvoidance(bool enable,
|
||||
limit_avoidance_gain_ = std::max(0.0, gain);
|
||||
limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
|
||||
limit_avoidance_max_push_ = max_push;
|
||||
|
||||
if (dls_solver_) {
|
||||
dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
|
||||
limit_avoidance_gain_,
|
||||
@ -456,26 +414,19 @@ void IbvsController::updateDepthControlPointInTag() {
|
||||
vpRotationMatrix R_des;
|
||||
R_des.buildFrom(target_rx_, target_ry_, target_rz_);
|
||||
|
||||
// 反解深度控制点（tag 平面点）:
|
||||
// 设控制点 p_o = [u, v, 0]^T，期望位姿为 (R_des, t_des)。
|
||||
// 在相机系:
|
||||
// p_c = R_des * p_o + t_des
|
||||
// 令控制点在期望时落在光轴上（x_c = 0, y_c = 0）:
|
||||
// [R00 R01] [u] = -[tx]
|
||||
// [R10 R11] [v] [ty]
|
||||
// 即 A * [u v]^T = b。
|
||||
Eigen::Matrix2d A;
|
||||
A << R_des[0][0], R_des[0][1],
|
||||
R_des[1][0], R_des[1][1];
|
||||
const Eigen::Vector2d b(-target_x_, -target_y_);
|
||||
|
||||
if (std::abs(A.determinant()) < 1e-9) {
|
||||
// 退化时回退到 tag 中心。
|
||||
depth_control_point_tag_.setZero();
|
||||
return;
|
||||
}
|
||||
|
||||
depth_control_point_tag_ = A.fullPivLu().solve(b);
|
||||
// 控制点限制在 tag 边界内，避免采样到背景。
|
||||
|
||||
// 限制在 tag 边界内
|
||||
depth_control_point_tag_.x() = SupportFunctions::clamp(depth_control_point_tag_.x(), -tag_half_, tag_half_);
|
||||
depth_control_point_tag_.y() = SupportFunctions::clamp(depth_control_point_tag_.y(), -tag_half_, tag_half_);
|
||||
}
|
||||
@ -487,14 +438,13 @@ void IbvsController::initTask() {
|
||||
task_->setLambda(lambda_);
|
||||
|
||||
obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
|
||||
obj_pts_[1].setWorldCoordinates(tag_half_, -tag_half_, 0.0);
|
||||
obj_pts_[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) {
|
||||
|
||||
@ -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",
|
||||
|
||||
@ -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})
|
||||
|
||||
281
cmvr-es/perception/include/apriltag_perception.h
Normal file
281
cmvr-es/perception/include/apriltag_perception.h
Normal file
@ -0,0 +1,281 @@
|
||||
//
|
||||
// Created by lgv on 2026/3/5.
|
||||
//
|
||||
|
||||
#pragma once
|
||||
#ifndef CMVR_PERCEPTION_APRILTAG_PERCEPTION_H
|
||||
#define CMVR_PERCEPTION_APRILTAG_PERCEPTION_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <visp3/core/vpCameraParameters.h>
|
||||
#include <visp3/core/vpHomogeneousMatrix.h>
|
||||
#include <visp3/core/vpImage.h>
|
||||
#include <visp3/detection/vpDetectorAprilTag.h>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
|
||||
namespace cmvr::perception {
|
||||
|
||||
/**
|
||||
* @brief AprilTag 视觉前端：统一抓帧、缓存派生图像并输出 AprilTag 检测结果。
|
||||
*
|
||||
* 坐标系约定：
|
||||
* - `c`：ViSP 相机坐标系，也是本类输出 `cMo` / `T_c_t` 所在的相机坐标系。
|
||||
* - `t`：AprilTag 自身坐标系，原点位于 tag 中心，z 轴垂直于 tag 平面。
|
||||
*
|
||||
* 使用约定：
|
||||
* - 每周期调用一次 `update()`；
|
||||
* - 其他模块（如 `TagRelativeTarget3D` / `IbvsController`）只读取缓存，不重复抓帧。
|
||||
*/
|
||||
class AprilTagPerception {
|
||||
public:
|
||||
enum class Status {
|
||||
OK = 0,
|
||||
NO_CAMERA,
|
||||
NO_NEW_FRAME,
|
||||
BAD_IMAGE,
|
||||
INVALID_INTRINSICS,
|
||||
NO_TAG,
|
||||
NO_DEPTH,
|
||||
ENCODED_UNAVAILABLE
|
||||
};
|
||||
|
||||
enum class DepthPolicy {
|
||||
// 只取 RGB。
|
||||
NONE = 0,
|
||||
|
||||
// 优先取 RGBD；失败或无深度时回退到 RGB。
|
||||
PREFER,
|
||||
|
||||
// 必须取 RGBD 且深度有效，否则本次更新失败。
|
||||
REQUIRE
|
||||
};
|
||||
|
||||
struct Options {
|
||||
// 深度抓取策略。
|
||||
DepthPolicy depth_policy{DepthPolicy::NONE};
|
||||
|
||||
// 是否执行 AprilTag 检测。
|
||||
bool detect_tags{true};
|
||||
|
||||
// 是否抓取编码帧（StreamFrameData.rgbFrame / depthFrame）。
|
||||
bool fetch_encoded{false};
|
||||
|
||||
// 抓取编码帧时传入相机驱动的索引。
|
||||
size_t encoded_index{0};
|
||||
};
|
||||
|
||||
struct Tag {
|
||||
// AprilTag id。
|
||||
int id{-1};
|
||||
|
||||
// AprilTag 检测的 decision margin，值越大通常表示检测越稳定。
|
||||
double margin{1.0};
|
||||
|
||||
// ViSP 表示的 tag 位姿：tag 坐标系 `t` 相对于 ViSP 相机坐标系 `c` 的位姿。
|
||||
vpHomogeneousMatrix cMo;
|
||||
|
||||
// Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
|
||||
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
|
||||
};
|
||||
|
||||
struct FrameCache {
|
||||
// 原始流缓存，包含 RGB / depth / 编码帧 / 内参等信息。
|
||||
device::StreamFrameData stream;
|
||||
|
||||
// 由 RGB 图转换得到的灰度图。
|
||||
cv::Mat gray;
|
||||
|
||||
// 当前 `gray` 是否有效。
|
||||
bool has_gray{false};
|
||||
|
||||
// ViSP 灰度图缓存，避免每帧重新分配。
|
||||
vpImage<unsigned char> visp_I;
|
||||
|
||||
// 当前 `visp_I` 是否有效。
|
||||
bool has_visp{false};
|
||||
|
||||
// `visp_I` 当前缓存的图像宽度。
|
||||
int visp_w{0};
|
||||
|
||||
// `visp_I` 当前缓存的图像高度。
|
||||
int visp_h{0};
|
||||
|
||||
// 本类内部维护的帧序号，每次 `update()` 成功后自增。
|
||||
uint64_t frame_id{0};
|
||||
|
||||
void clearDerived() {
|
||||
gray.release();
|
||||
has_gray = false;
|
||||
// visp_I 保留分配复用
|
||||
has_visp = false;
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief 构造感知前端。
|
||||
* @param camera 相机对象；可为空，后续再通过 `setCamera()` 注入。
|
||||
*/
|
||||
explicit AprilTagPerception(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
|
||||
|
||||
/**
|
||||
* @brief 设置相机对象，并清空当前缓存与检测结果。
|
||||
* @param camera 相机对象。
|
||||
*/
|
||||
void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera);
|
||||
const std::shared_ptr<cmvr::device::AbstractCamera>& camera() const { return camera_; }
|
||||
|
||||
/**
|
||||
* @brief 设置 tag 物理边长。
|
||||
* @param tag_size_m tag 边长，单位米。
|
||||
*/
|
||||
void setTagSize(double tag_size_m);
|
||||
double tagSize() const { return tag_size_m_; }
|
||||
|
||||
/**
|
||||
* @brief 设置 AprilTag 家族。
|
||||
* @param fam AprilTag 家族类型。
|
||||
*/
|
||||
void setTagFamily(vpDetectorAprilTag::vpAprilTagFamily fam);
|
||||
|
||||
/**
|
||||
* @brief 每周期调用一次：抓帧（RGB/RGBD，编码帧可选）+ detect（可选）+ 缓存结果。
|
||||
* @return 成功返回 true。
|
||||
*/
|
||||
bool update(const Options& opt);
|
||||
|
||||
/**
|
||||
* @brief 获取最近一次 `update()` 的状态。
|
||||
* @return 最近一次状态枚举。
|
||||
*/
|
||||
Status lastStatus() const { return last_status_; }
|
||||
|
||||
/**
|
||||
* @brief 状态枚举转字符串。
|
||||
* @param s 状态枚举。
|
||||
* @return 状态字符串。
|
||||
*/
|
||||
static const char* statusToString(Status s);
|
||||
|
||||
/**
|
||||
* @brief 获取当前缓存对应的帧序号。
|
||||
* @return 帧序号。
|
||||
*/
|
||||
uint64_t frameId() const { return frame_.frame_id; }
|
||||
|
||||
// ---- frame getters ----
|
||||
// 完整帧缓存。
|
||||
const FrameCache& frame() const { return frame_; }
|
||||
|
||||
// 原始流缓存。
|
||||
const device::StreamFrameData& stream() const { return frame_.stream; }
|
||||
|
||||
// 当前 RGB 图。
|
||||
const cv::Mat& color() const { return frame_.stream.rgbImage; }
|
||||
|
||||
// 当前深度图。
|
||||
const cv::Mat& depth() const { return frame_.stream.depthImage; }
|
||||
|
||||
// 当前灰度图。
|
||||
const cv::Mat& gray() const { return frame_.gray; }
|
||||
|
||||
// 当前相机内参。
|
||||
const device::Rs2Intrinsics& intrinsics() const { return frame_.stream.intrinsics; }
|
||||
|
||||
// ViSP 相机模型参数。
|
||||
const vpCameraParameters& vispCamera() const { return visp_cam_; }
|
||||
|
||||
// 当前缓存是否包含深度图。
|
||||
bool hasDepth() const { return !frame_.stream.depthImage.empty(); }
|
||||
|
||||
// ---- detection outputs ----
|
||||
// 当前缓存中是否包含至少一个检测到的 tag。
|
||||
bool hasTags() const { return !tags_.empty(); }
|
||||
|
||||
// 当前帧的 tag 检测结果。
|
||||
const std::vector<Tag>& tags() const { return tags_; }
|
||||
|
||||
/**
|
||||
* @brief 按 id 查找当前帧检测到的 tag。
|
||||
* @param id tag id。
|
||||
* @return 找到则返回指针，否则返回 `nullptr`。
|
||||
*/
|
||||
const Tag* findTag(int id) const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 按 `Options` 抓取当前帧的 RGB / depth 数据并填充 `frame_.stream`。
|
||||
* @param opt 本次更新使用的抓帧选项。
|
||||
* @return 抓帧成功返回 `true`。
|
||||
*/
|
||||
bool grabFrame(const Options& opt);
|
||||
|
||||
/**
|
||||
* @brief 抓取编码帧并写入 `frame_.stream`。
|
||||
* @param encoded_index 相机驱动使用的编码帧索引。
|
||||
* @return 抓取成功返回 `true`。
|
||||
*/
|
||||
bool fetchEncoded(size_t encoded_index);
|
||||
|
||||
/**
|
||||
* @brief 确保 `frame_.gray` 已由当前 RGB 图转换得到。
|
||||
* @return 灰度图可用返回 `true`。
|
||||
*/
|
||||
bool ensureGray();
|
||||
|
||||
/**
|
||||
* @brief 确保 `frame_.visp_I` 已由当前灰度图填充。
|
||||
* @return ViSP 灰度图可用返回 `true`。
|
||||
*/
|
||||
bool ensureVispImage();
|
||||
|
||||
/**
|
||||
* @brief 在当前帧上执行 AprilTag 检测，并填充 `tags_` / `id_to_index_`。
|
||||
* @return 检测到至少一个 tag 返回 `true`。
|
||||
*/
|
||||
bool detectTags();
|
||||
|
||||
/**
|
||||
* @brief 将 ViSP 位姿 `cMo` 转为 Eigen 齐次变换 `T_c_t`。
|
||||
* @param cMo tag 坐标系 `t` 相对于相机坐标系 `c` 的 ViSP 位姿。
|
||||
* @return 对应的 Eigen 齐次变换 `T_c_t`。
|
||||
*/
|
||||
static Eigen::Matrix4d toEigen4(const vpHomogeneousMatrix& cMo);
|
||||
|
||||
private:
|
||||
// 相机对象。
|
||||
std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
|
||||
|
||||
// AprilTag 检测器。
|
||||
vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
|
||||
|
||||
// tag 物理边长，单位米。
|
||||
double tag_size_m_{0.12};
|
||||
|
||||
// 与当前内参对应的 ViSP 相机模型。
|
||||
vpCameraParameters visp_cam_;
|
||||
|
||||
// 当前帧缓存。
|
||||
FrameCache frame_;
|
||||
|
||||
// 当前帧检测到的 tag 列表。
|
||||
std::vector<Tag> tags_;
|
||||
|
||||
// tag id 到 `tags_` 下标的映射。
|
||||
std::unordered_map<int, size_t> id_to_index_;
|
||||
|
||||
// 最近一次 `update()` 的状态。
|
||||
Status last_status_{Status::NO_NEW_FRAME};
|
||||
};
|
||||
|
||||
} // namespace cmvr::perception
|
||||
|
||||
#endif // CMVR_PERCEPTION_APRILTAG_PERCEPTION_H
|
||||
@ -2,124 +2,98 @@
|
||||
#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
|
||||
#define CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <visp3/detection/vpDetectorAprilTag.h>
|
||||
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
#include "perception/include/apriltag_perception.h"
|
||||
|
||||
namespace cmvr::perception {
|
||||
|
||||
/**
|
||||
* @brief Tag 相对目标 3D 跟踪器（全封装版）。
|
||||
* @brief 利用 AprilTag 观测跟踪一个目标点的 3D 位置。
|
||||
*
|
||||
* 该类内部完成：
|
||||
* 1) 从相机抓取 RGB 或 RGBD；
|
||||
* 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<cmvr::device::AbstractCamera>& camera = nullptr);
|
||||
~TagRelativeTarget3D() = default;
|
||||
explicit TagRelativeTarget3D(const std::shared_ptr<AprilTagPerception>& perception = nullptr);
|
||||
|
||||
/**
|
||||
* @brief 设置/替换相机对象。
|
||||
* @param camera 抽象相机对象。
|
||||
* @brief 设置共享感知前端。
|
||||
* @param perception 感知前端。
|
||||
*/
|
||||
void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera);
|
||||
void setPerception(const std::shared_ptr<AprilTagPerception>& perception) { perception_ = perception; }
|
||||
const std::shared_ptr<AprilTagPerception>& perception() const { return perception_; }
|
||||
|
||||
/**
|
||||
* @brief 清空内部状态（锚点、缓存、追踪状态与最近输出）。
|
||||
* @brief 清空所有已锁定的目标锚点与跟踪状态。
|
||||
*/
|
||||
void clear();
|
||||
|
||||
/**
|
||||
* @brief 是否没有任何已缓存锚点。
|
||||
* @return 没有锚点返回 `true`。
|
||||
*/
|
||||
// 当前是否还没有任何已锁定锚点。
|
||||
bool empty() const { return anchors_in_tag_.empty(); }
|
||||
|
||||
/**
|
||||
* @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<AprilTagPerception> perception_{nullptr};
|
||||
|
||||
// 当前使用的感知帧 id。
|
||||
uint64_t used_frame_id_{0};
|
||||
|
||||
// 当前帧相机内参。
|
||||
device::Rs2Intrinsics intr_{};
|
||||
|
||||
// 指向当前帧深度图的只读引用。
|
||||
const cv::Mat* depth_{nullptr};
|
||||
|
||||
// 当前帧 tag 观测列表。
|
||||
std::vector<Observation> observations_;
|
||||
|
||||
// 目标点锚点表：`tag_id -> p_t_target`，其中 `p_t_target` 位于对应 tag 坐标系 `t`。
|
||||
std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_;
|
||||
|
||||
// 相机对象。
|
||||
std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
|
||||
|
||||
// AprilTag 检测器。
|
||||
vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
|
||||
// AprilTag 物理边长（米）。
|
||||
double tag_size_m_{0.12};
|
||||
|
||||
// 最近一次调用状态。
|
||||
// 最近一次接口调用的状态。
|
||||
Status last_status_{Status::TARGET_NOT_LOCKED};
|
||||
|
||||
// 图像/内参缓存。
|
||||
FrameCache frame_;
|
||||
// 当前帧 tag 观测缓存。
|
||||
std::vector<TagPoseObservation> observations_;
|
||||
// 观测缓存对应的帧 id。
|
||||
uint64_t obs_frame_id_{0};
|
||||
|
||||
// 当前 active tag id。
|
||||
int active_tag_id_{-1};
|
||||
// active tag 连续丢失帧计数。
|
||||
|
||||
// 当前 active tag 连续丢失的帧数。
|
||||
int active_tag_missing_count_{0};
|
||||
// 连续丢失多少帧后允许切换 active tag。
|
||||
|
||||
// active tag 丢失多少帧后允许切换。
|
||||
int missing_before_switch_{4};
|
||||
// active 切换迟滞阈值。
|
||||
|
||||
// active tag 切换迟滞系数。
|
||||
double switch_hysteresis_{1.2};
|
||||
|
||||
// 候选评分中权重项指数。
|
||||
// 候选评分中的观测权重指数。
|
||||
double candidate_weight_power_{1.0};
|
||||
// 候选评分中距离项指数。
|
||||
|
||||
// 候选评分中的邻近项指数。
|
||||
double candidate_proximity_power_{1.0};
|
||||
// 候选评分距离归一化尺度（米）。
|
||||
|
||||
// 候选评分中的邻近项归一化尺度，单位米，位于 tag 坐标系 `t`。
|
||||
double candidate_proximity_scale_m_{0.08};
|
||||
|
||||
// 当前像素转 3D 方法。
|
||||
// 目标点恢复方式。
|
||||
TargetPointMethod target_point_method_{TargetPointMethod::TAG_PLANE};
|
||||
|
||||
// 深度采样窗口边长（像素）。
|
||||
// 深度采样窗口边长，单位像素。
|
||||
int depth_window_size_px_{3};
|
||||
// 是否启用深度离群剔除。
|
||||
|
||||
// 是否启用深度离群点剔除。
|
||||
bool depth_outlier_reject_enabled_{true};
|
||||
// 深度离群剔除 sigma 参数。
|
||||
|
||||
// 深度离群点剔除阈值，单位标准差倍数。
|
||||
double depth_outlier_sigma_{2.5};
|
||||
// 深度离群剔除最小绝对偏差（米）。
|
||||
|
||||
// 深度离群点剔除最小偏差，单位米。
|
||||
double depth_outlier_min_dev_m_{0.002};
|
||||
|
||||
// 最近一次输出的目标点相机坐标。
|
||||
// 最近一次解算/跟踪得到的目标点，位于相机坐标系 `c`。
|
||||
Eigen::Vector3d last_p_c_target_{Eigen::Vector3d::Zero()};
|
||||
// 最近一次输出使用的主导 tag id。
|
||||
|
||||
// 最近一次结果实际使用的 tag id。
|
||||
int last_used_tag_id_{-1};
|
||||
// 最近一次输出离散度（米）。
|
||||
|
||||
// 最近一次多 tag 融合结果的离散度，单位米。
|
||||
double last_spread_m_{0.0};
|
||||
// 最近一次 track 是否发生 active tag 切换。
|
||||
|
||||
// 最近一次 `track()` 是否发生了 active tag 切换。
|
||||
bool last_switched_{false};
|
||||
};
|
||||
|
||||
|
||||
286
cmvr-es/perception/src/apriltag_perception.cpp
Normal file
286
cmvr-es/perception/src/apriltag_perception.cpp
Normal file
@ -0,0 +1,286 @@
|
||||
//
|
||||
// Created by lgv on 2026/3/5.
|
||||
//
|
||||
|
||||
#include "perception/include/apriltag_perception.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace cmvr::perception {
|
||||
|
||||
AprilTagPerception::AprilTagPerception(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
|
||||
: camera_(camera) {
|
||||
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
|
||||
}
|
||||
|
||||
void AprilTagPerception::setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) {
|
||||
camera_ = camera;
|
||||
tags_.clear();
|
||||
id_to_index_.clear();
|
||||
frame_.stream = device::StreamFrameData{};
|
||||
frame_.clearDerived();
|
||||
frame_.frame_id = 0;
|
||||
last_status_ = Status::NO_NEW_FRAME;
|
||||
}
|
||||
|
||||
void AprilTagPerception::setTagSize(double tag_size_m) {
|
||||
if (std::isfinite(tag_size_m) && tag_size_m > 0.0) {
|
||||
tag_size_m_ = tag_size_m;
|
||||
}
|
||||
}
|
||||
|
||||
void AprilTagPerception::setTagFamily(vpDetectorAprilTag::vpAprilTagFamily fam) {
|
||||
detector_ = vpDetectorAprilTag(fam);
|
||||
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
|
||||
}
|
||||
|
||||
const char* AprilTagPerception::statusToString(Status s) {
|
||||
switch (s) {
|
||||
case Status::OK: return "ok";
|
||||
case Status::NO_CAMERA: return "no_camera";
|
||||
case Status::NO_NEW_FRAME: return "no_new_frame";
|
||||
case Status::BAD_IMAGE: return "bad_image";
|
||||
case Status::INVALID_INTRINSICS: return "invalid_intrinsics";
|
||||
case Status::NO_TAG: return "no_tag";
|
||||
case Status::NO_DEPTH: return "no_depth";
|
||||
case Status::ENCODED_UNAVAILABLE: return "encoded_unavailable";
|
||||
default: return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
bool AprilTagPerception::update(const Options& opt) {
|
||||
tags_.clear();
|
||||
id_to_index_.clear();
|
||||
frame_.clearDerived();
|
||||
|
||||
if (!grabFrame(opt)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// intrinsics -> visp camera params
|
||||
const double fx = static_cast<double>(frame_.stream.intrinsics.fx);
|
||||
const double fy = static_cast<double>(frame_.stream.intrinsics.fy);
|
||||
const double cx = static_cast<double>(frame_.stream.intrinsics.cx);
|
||||
const double cy = static_cast<double>(frame_.stream.intrinsics.cy);
|
||||
|
||||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
|
||||
last_status_ = Status::INVALID_INTRINSICS;
|
||||
return false;
|
||||
}
|
||||
visp_cam_.initPersProjWithoutDistortion(fx, fy, cx, cy);
|
||||
|
||||
// optional: fetch encoded (after we have the newest frame)
|
||||
if (opt.fetch_encoded) {
|
||||
if (!fetchEncoded(opt.encoded_index)) {
|
||||
// 编码帧抓取失败不一定要致命（看你喜好）
|
||||
// 这里按“失败但继续 detect/输出 Mat”处理，只设置状态供外部看
|
||||
last_status_ = Status::ENCODED_UNAVAILABLE;
|
||||
// 注意：不 return false，让 detect 还能跑；你也可以改成 return false
|
||||
}
|
||||
}
|
||||
|
||||
// detect tags
|
||||
if (opt.detect_tags) {
|
||||
if (!ensureVispImage()) return false;
|
||||
if (!detectTags()) return false;
|
||||
} else {
|
||||
last_status_ = Status::OK;
|
||||
}
|
||||
|
||||
frame_.frame_id++;
|
||||
return (last_status_ == Status::OK);
|
||||
}
|
||||
|
||||
bool AprilTagPerception::grabFrame(const Options& opt) {
|
||||
if (!camera_) {
|
||||
last_status_ = Status::NO_CAMERA;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 清空 stream 中 Mat，但保留结构体可复用
|
||||
frame_.stream.rgbImage.release();
|
||||
frame_.stream.depthImage.release();
|
||||
frame_.stream.rgbFrame.clear();
|
||||
frame_.stream.depthFrame.clear();
|
||||
frame_.stream.bKey = false;
|
||||
frame_.stream.depthKey = false;
|
||||
|
||||
try {
|
||||
if (opt.depth_policy == DepthPolicy::NONE) {
|
||||
camera_->getRGBImage(frame_.stream.rgbImage, frame_.stream.intrinsics);
|
||||
} else {
|
||||
camera_->getRGBDImages(frame_.stream.rgbImage, frame_.stream.depthImage, frame_.stream.intrinsics);
|
||||
}
|
||||
} catch (...) {
|
||||
// PREFER: 允许回退 RGB
|
||||
if (opt.depth_policy == DepthPolicy::PREFER) {
|
||||
try {
|
||||
camera_->getRGBImage(frame_.stream.rgbImage, frame_.stream.intrinsics);
|
||||
frame_.stream.depthImage.release();
|
||||
} catch (...) {
|
||||
last_status_ = Status::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
last_status_ = Status::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (frame_.stream.rgbImage.empty()) {
|
||||
last_status_ = Status::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
|
||||
// basic image sanity
|
||||
const int ch = frame_.stream.rgbImage.channels();
|
||||
if (ch != 1 && ch != 3 && ch != 4) {
|
||||
last_status_ = Status::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
|
||||
// depth requirement
|
||||
if (opt.depth_policy == DepthPolicy::REQUIRE) {
|
||||
if (frame_.stream.depthImage.empty()) {
|
||||
last_status_ = Status::NO_DEPTH;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
last_status_ = Status::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AprilTagPerception::fetchEncoded(size_t encoded_index) {
|
||||
if (!camera_) {
|
||||
last_status_ = Status::NO_CAMERA;
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
size_t idx = encoded_index;
|
||||
camera_->getEncodedFrame(frame_.stream, idx);
|
||||
// 注意：这里 frame_.stream.rgbFrame / depthFrame / codec / bKey 等由相机实现填充
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool AprilTagPerception::ensureGray() {
|
||||
if (frame_.has_gray && !frame_.gray.empty()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const cv::Mat& color = frame_.stream.rgbImage;
|
||||
if (color.empty()) {
|
||||
last_status_ = Status::NO_NEW_FRAME;
|
||||
return false;
|
||||
}
|
||||
|
||||
const int ch = color.channels();
|
||||
if (ch == 1) {
|
||||
frame_.gray = color;
|
||||
} else if (ch == 3) {
|
||||
cv::cvtColor(color, frame_.gray, cv::COLOR_BGR2GRAY);
|
||||
} else if (ch == 4) {
|
||||
cv::cvtColor(color, frame_.gray, cv::COLOR_BGRA2GRAY);
|
||||
} else {
|
||||
last_status_ = Status::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (frame_.gray.empty() || frame_.gray.type() != CV_8UC1) {
|
||||
last_status_ = Status::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!frame_.gray.isContinuous()) {
|
||||
frame_.gray = frame_.gray.clone();
|
||||
}
|
||||
|
||||
frame_.has_gray = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AprilTagPerception::ensureVispImage() {
|
||||
if (!ensureGray()) return false;
|
||||
|
||||
const int w = frame_.gray.cols;
|
||||
const int h = frame_.gray.rows;
|
||||
|
||||
if (!frame_.has_visp || frame_.visp_w != w || frame_.visp_h != h) {
|
||||
frame_.visp_I.resize(h, w);
|
||||
frame_.visp_w = w;
|
||||
frame_.visp_h = h;
|
||||
frame_.has_visp = true;
|
||||
}
|
||||
|
||||
for (int y = 0; y < h; ++y) {
|
||||
std::memcpy(frame_.visp_I[y],
|
||||
frame_.gray.ptr<unsigned char>(y),
|
||||
static_cast<size_t>(w));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AprilTagPerception::detectTags() {
|
||||
std::vector<vpHomogeneousMatrix> cMo_vec;
|
||||
const bool detected = detector_.detect(frame_.visp_I, tag_size_m_, visp_cam_, cMo_vec);
|
||||
if (!detected || cMo_vec.empty()) {
|
||||
last_status_ = Status::NO_TAG;
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::vector<int> tag_ids = detector_.getTagsId();
|
||||
const std::vector<float> margins = detector_.getTagsDecisionMargin();
|
||||
const size_t n = std::min(tag_ids.size(), cMo_vec.size());
|
||||
if (n == 0) {
|
||||
last_status_ = Status::NO_TAG;
|
||||
return false;
|
||||
}
|
||||
|
||||
tags_.reserve(n);
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
Tag t;
|
||||
t.id = tag_ids[i];
|
||||
t.cMo = cMo_vec[i];
|
||||
t.T_c_t = toEigen4(cMo_vec[i]);
|
||||
|
||||
double w = 1.0;
|
||||
if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
|
||||
w = static_cast<double>(margins[i]);
|
||||
}
|
||||
t.margin = w;
|
||||
|
||||
id_to_index_[t.id] = tags_.size();
|
||||
tags_.push_back(t);
|
||||
}
|
||||
|
||||
last_status_ = Status::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
const AprilTagPerception::Tag* AprilTagPerception::findTag(int id) const {
|
||||
const auto it = id_to_index_.find(id);
|
||||
if (it == id_to_index_.end()) return nullptr;
|
||||
return &tags_[it->second];
|
||||
}
|
||||
|
||||
Eigen::Matrix4d AprilTagPerception::toEigen4(const vpHomogeneousMatrix& cMo) {
|
||||
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||
for (int r = 0; r < 3; ++r) {
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
T(r, c) = cMo[r][c];
|
||||
}
|
||||
}
|
||||
T(0, 3) = cMo[0][3];
|
||||
T(1, 3) = cMo[1][3];
|
||||
T(2, 3) = cMo[2][3];
|
||||
return T;
|
||||
}
|
||||
|
||||
} // namespace cmvr::perception
|
||||
@ -2,26 +2,14 @@
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <visp3/core/vpCameraParameters.h>
|
||||
#include <visp3/core/vpHomogeneousMatrix.h>
|
||||
#include <visp3/core/vpImage.h>
|
||||
|
||||
#include "common/utils/image/image_process.h"
|
||||
|
||||
namespace cmvr::perception {
|
||||
namespace {
|
||||
|
||||
// -------- helpers --------
|
||||
double sanitizeWeight(double w) {
|
||||
if (!std::isfinite(w) || w <= 0.0) return 1.0;
|
||||
return w;
|
||||
}
|
||||
|
||||
class WeightedPointFusion {
|
||||
public:
|
||||
void reserve(size_t n) { points_.reserve(n); }
|
||||
@ -39,10 +27,8 @@ public:
|
||||
|
||||
bool finalize(Eigen::Vector3d& p_c_out, int& best_id_out, double& spread_out) const {
|
||||
if (points_.empty() || weight_sum_ <= 0.0) return false;
|
||||
|
||||
p_c_out = weighted_sum_ / weight_sum_;
|
||||
best_id_out = best_id_;
|
||||
|
||||
double max_err = 0.0;
|
||||
for (const auto& p : points_) {
|
||||
max_err = std::max(max_err, (p - p_c_out).norm());
|
||||
@ -61,16 +47,10 @@ private:
|
||||
|
||||
} // namespace
|
||||
|
||||
TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
|
||||
: camera_(camera) {
|
||||
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
|
||||
}
|
||||
TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<AprilTagPerception>& perception)
|
||||
: perception_(perception) {}
|
||||
|
||||
void TagRelativeTarget3D::setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& 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<double>(frame_.intr.fx);
|
||||
const double fy = static_cast<double>(frame_.intr.fy);
|
||||
const double cx = static_cast<double>(frame_.intr.cx);
|
||||
const double cy = static_cast<double>(frame_.intr.cy);
|
||||
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
|
||||
last_status_ = Status::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
|
||||
cv::Mat gray;
|
||||
if (color.channels() == 1) gray = color;
|
||||
else if (color.channels() == 3) cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
|
||||
else cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
|
||||
|
||||
if (gray.empty() || gray.type() != CV_8UC1) {
|
||||
last_status_ = Status::BAD_IMAGE;
|
||||
return false;
|
||||
}
|
||||
if (!gray.isContinuous()) gray = gray.clone();
|
||||
|
||||
const int width = gray.cols;
|
||||
const int height = gray.rows;
|
||||
|
||||
vpCameraParameters cam;
|
||||
cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
|
||||
|
||||
vpImage<unsigned char> I(height, width);
|
||||
for (int y = 0; y < height; ++y) {
|
||||
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
|
||||
}
|
||||
|
||||
std::vector<vpHomogeneousMatrix> cMo_vec;
|
||||
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
|
||||
if (!detected || cMo_vec.empty()) {
|
||||
last_status_ = Status::NO_TAG;
|
||||
obs_frame_id_ = frame_.frame_id;
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::vector<int> tag_ids = detector_.getTagsId();
|
||||
const std::vector<float> margins = detector_.getTagsDecisionMargin();
|
||||
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
|
||||
if (pair_size == 0) {
|
||||
last_status_ = Status::NO_TAG;
|
||||
obs_frame_id_ = frame_.frame_id;
|
||||
return false;
|
||||
}
|
||||
|
||||
observations_.clear();
|
||||
observations_.reserve(pair_size);
|
||||
|
||||
for (size_t i = 0; i < pair_size; ++i) {
|
||||
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||
const vpHomogeneousMatrix& cMo = cMo_vec[i];
|
||||
|
||||
for (int r = 0; r < 3; ++r) {
|
||||
for (int c = 0; c < 3; ++c) T(r, c) = cMo[r][c];
|
||||
}
|
||||
T(0, 3) = cMo[0][3];
|
||||
T(1, 3) = cMo[1][3];
|
||||
T(2, 3) = cMo[2][3];
|
||||
|
||||
double w = 1.0;
|
||||
if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
|
||||
w = static_cast<double>(margins[i]);
|
||||
}
|
||||
|
||||
observations_.push_back(TagPoseObservation{tag_ids[i], T, w});
|
||||
}
|
||||
|
||||
if (observations_.empty()) {
|
||||
last_status_ = Status::NO_TAG;
|
||||
obs_frame_id_ = frame_.frame_id;
|
||||
return false;
|
||||
}
|
||||
|
||||
obs_frame_id_ = frame_.frame_id;
|
||||
last_status_ = Status::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================
|
||||
// Public APIs (u,v only)
|
||||
// =========================
|
||||
bool TagRelativeTarget3D::solveFromPixel(int u, int v) {
|
||||
resetLastOutputs();
|
||||
|
||||
if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) {
|
||||
if (!updateInternal(true, false)) return false; // RGBD only
|
||||
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<double>::infinity();
|
||||
@ -565,7 +361,6 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p
|
||||
active_tag_id_ = active_tag;
|
||||
active_tag_missing_count_ = 0;
|
||||
|
||||
// 输出保持 lock 点
|
||||
last_p_c_target_ = p_c_lock;
|
||||
last_used_tag_id_ = used_tag;
|
||||
last_spread_m_ = spread;
|
||||
@ -574,12 +369,7 @@ bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TagRelativeTarget3D::trackWithActiveTagFromCached() {
|
||||
if (observations_.empty()) {
|
||||
last_status_ = Status::NO_OBSERVATION;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool TagRelativeTarget3D::trackWithActiveTag() {
|
||||
int active_obs_index = -1;
|
||||
double active_score = -std::numeric_limits<double>::infinity();
|
||||
|
||||
@ -647,44 +437,25 @@ bool TagRelativeTarget3D::trackWithActiveTagFromCached() {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// 动态补 p_t_target
|
||||
lockAnchorsFromPoint(p_c, false);
|
||||
|
||||
last_p_c_target_ = p_c;
|
||||
last_used_tag_id_ = obs.tag_id;
|
||||
last_spread_m_ = 0.0; // 单 tag
|
||||
last_spread_m_ = 0.0;
|
||||
last_switched_ = switched;
|
||||
last_status_ = Status::OK;
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================
|
||||
// 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<double>::infinity();
|
||||
|
||||
const auto it = anchors_in_tag_.find(obs.tag_id);
|
||||
if (it == anchors_in_tag_.end()) return -std::numeric_limits<double>::infinity();
|
||||
|
||||
const double w = std::max(1e-12, sanitizeObsWeight(obs.weight));
|
||||
const double w = std::max(1e-12, sanitizeWeight(obs.weight));
|
||||
const Eigen::Vector3d& p_t_target = it->second;
|
||||
|
||||
const double dist_in_tag = std::hypot(p_t_target.x(), p_t_target.y());
|
||||
const double proximity_gain = 1.0 / (1.0 + dist_in_tag / candidate_proximity_scale_m_);
|
||||
|
||||
@ -696,6 +467,12 @@ double TagRelativeTarget3D::trackingCandidateScore(const TagPoseObservation& obs
|
||||
return score;
|
||||
}
|
||||
|
||||
// -------- math helpers --------
|
||||
double TagRelativeTarget3D::sanitizeWeight(double w) {
|
||||
if (!std::isfinite(w) || w <= 0.0) return 1.0;
|
||||
return w;
|
||||
}
|
||||
|
||||
bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) {
|
||||
for (int r = 0; r < 4; ++r)
|
||||
for (int c = 0; c < 4; ++c)
|
||||
@ -708,29 +485,29 @@ bool TagRelativeTarget3D::isFiniteVector(const Eigen::Vector3d& p) {
|
||||
}
|
||||
|
||||
Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t,
|
||||
const Eigen::Vector3d& p_t) {
|
||||
const Eigen::Vector3d& p_t) {
|
||||
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
|
||||
const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
|
||||
return R * p_t + t;
|
||||
}
|
||||
|
||||
Eigen::Vector3d TagRelativeTarget3D::pointCameraToTag(const Eigen::Matrix4d& T_c_t,
|
||||
const Eigen::Vector3d& p_c) {
|
||||
const Eigen::Vector3d& p_c) {
|
||||
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
|
||||
const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
|
||||
return R.transpose() * (p_c - t);
|
||||
}
|
||||
|
||||
bool TagRelativeTarget3D::intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
|
||||
const Eigen::Vector3d& ray_c,
|
||||
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;
|
||||
|
||||
@ -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<cmvr::device::RealsenseCamera>(cam_cfg);
|
||||
cmvr::perception::TagRelativeTarget3D tracker(camera);
|
||||
tracker.setTagSize(tag_size);
|
||||
|
||||
auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
|
||||
perception->setTagSize(tag_size);
|
||||
cmvr::perception::TagRelativeTarget3D tracker(perception);
|
||||
|
||||
cmvr::perception::AprilTagPerception::Options opt;
|
||||
opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
|
||||
opt.detect_tags = true;
|
||||
tracker.setActiveTagSwitchPolicy(4, 1.2);
|
||||
tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
|
||||
tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
|
||||
@ -413,6 +419,7 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
|
||||
std::string phase = target_locked ? "track" : "lock";
|
||||
|
||||
if (!target_locked) {
|
||||
perception->update(opt);
|
||||
if (tracker.startTrackingFromPixel(target_u, target_v)) {
|
||||
p_c_target = tracker.lastTargetInCamera();
|
||||
used_tag_id = tracker.lastUsedTagId();
|
||||
@ -439,11 +446,13 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
|
||||
}
|
||||
} else {
|
||||
const int old_active_tag = tracker.activeTagId();
|
||||
perception->update(opt);
|
||||
if (tracker.track()) {
|
||||
p_c_target = tracker.lastTargetInCamera();
|
||||
used_tag_id = tracker.lastUsedTagId();
|
||||
spread_m = tracker.lastSpread();
|
||||
const bool switched = tracker.lastSwitched();
|
||||
|
||||
target_valid = true;
|
||||
ok = true;
|
||||
++track_success_count;
|
||||
@ -458,7 +467,8 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
|
||||
<< " used_tag=" << used_tag_id
|
||||
<< " active_tag=" << tracker.activeTagId()
|
||||
<< " spread=" << spread_m
|
||||
<< "\n";
|
||||
<< "anchorCount=" << tracker.anchorCount()
|
||||
<< std::endl;
|
||||
} else {
|
||||
++track_fail_count;
|
||||
if (((i + 1) % kLogEveryNFrames) == 0) {
|
||||
@ -595,109 +605,109 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
|
||||
<< "\n";
|
||||
}
|
||||
|
||||
TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) {
|
||||
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<cmvr::device::RealsenseCamera>(cam_cfg);
|
||||
cmvr::perception::TagRelativeTarget3D tracker(camera);
|
||||
ASSERT_NO_THROW(camera->init());
|
||||
ASSERT_NO_THROW(camera->start());
|
||||
struct CameraStopGuard {
|
||||
std::shared_ptr<cmvr::device::RealsenseCamera> cam;
|
||||
~CameraStopGuard() {
|
||||
if (!cam) return;
|
||||
try {
|
||||
cam->stop();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
} stop_guard{camera};
|
||||
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<cmvr::device::RealsenseCamera>(cam_cfg);
|
||||
// cmvr::perception::TagRelativeTarget3D tracker(camera);
|
||||
// ASSERT_NO_THROW(camera->init());
|
||||
// ASSERT_NO_THROW(camera->start());
|
||||
// struct CameraStopGuard {
|
||||
// std::shared_ptr<cmvr::device::RealsenseCamera> cam;
|
||||
// ~CameraStopGuard() {
|
||||
// if (!cam) return;
|
||||
// try {
|
||||
// cam->stop();
|
||||
// } catch (...) {
|
||||
// }
|
||||
// }
|
||||
// } stop_guard{camera};
|
||||
// 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;
|
||||
// }
|
||||
|
||||
@ -66,7 +66,7 @@
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<body name="tag_board" pos="0.7 -0.2 1.05" euler="1.37 -1.57 0">
|
||||
<body name="tag_board" pos="0.7 -0.2 1.05" euler="1.57 -1.57 0">
|
||||
|
||||
<!-- 15cm x 15cm 白板，同时贴 apriltag 纹理（纹理里自带白边） -->
|
||||
<geom name="tag_board_geom"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user