diff --git a/README.md b/README.md index 2b54e6e8..4e1597a9 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ ## Installation -### 1. Git submodules install +### 1. Git submodules install ``` git submodule update --init --recursive @@ -15,14 +15,14 @@ git submodule update --init --recursive ```shell # basic sudo apt-get update -sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev +sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev + # opencv sudo apt install -y \ libjpeg-dev libpng-dev libtiff-dev \ libavcodec-dev libavformat-dev libswscale-dev \ libv4l-dev libxvidcore-dev libx264-dev \ - libgtk-3-dev - + libgtk-3-dev # PulseAudio sudo apt-get install libpulse-dev @@ -37,252 +37,10 @@ sudo apt-get install libglfw3-dev sudo apt-get install libassimp-dev # visp -sudo apt-get install -y libx11-dev liblapack-dev libv4l-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev +sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev -[//]: # () -[//]: # (# json) - -[//]: # (sudo apt install libjsoncpp-dev -y) - -[//]: # () -[//]: # () -[//]: # () -[//]: # () -[//]: # () -[//]: # () -[//]: # () -[//]: # () -[//]: # (# flann) - -[//]: # (sudo apt-get install libflann-dev -y) - -[//]: # () -[//]: # () -[//]: # () -[//]: # (#alas) - -[//]: # (sudo apt-get install libasound2-dev -y) - -[//]: # () -[//]: # (# gstreamer) - -[//]: # (sudo apt install gstreamer1.0-plugins-good gstreamer1.0-plugins-bad -y) - -[//]: # (sudo apt install gstreamer1.0-plugins-ugly gstreamer1.0-libav -y) - -[//]: # () -[//]: # (# tinyxml2) - -[//]: # (sudo apt install libtinyxml2-dev -y) - -[//]: # () -[//]: # () -[//]: # (```) - -[//]: # () -[//]: # (### 3. gRPC install (build from source)) - -[//]: # (version: https://grpc.io/docs/languages/cpp/quickstart/（grpc官网地址 v1.73.0）) - -[//]: # (#### 安装完成之后要配置环境变量) - -[//]: # (vim ~/.bashrc) - -[//]: # (在末尾添加) - -[//]: # () -[//]: # (### 4. ffmpeg install (build from source)) - -[//]: # (#### 4.1 install dependencies) - -[//]: # (```shell) - -[//]: # (sudo apt-get update) - -[//]: # (sudo apt install build-essential yasm nasm git -y) - -[//]: # (sudo apt install libx264-dev libx265-dev libvpx-dev libfdk-aac-dev libmp3lame-dev libopus-dev -y) - -[//]: # (```) - -[//]: # () -[//]: # (#### 4.2 build from source) - -[//]: # (```shell) - -[//]: # (cd assets) - -[//]: # (tar xjvf ffmpeg-4.4.tar.bz2) - -[//]: # (cd ffmpeg-4.4) - -[//]: # () -[//]: # (# set configurations) - -[//]: # (./configure \) - -[//]: # ( --extra-libs="-lpthread -lm" \) - -[//]: # ( --enable-gpl \) - -[//]: # ( --enable-libass \) - -[//]: # ( --enable-libfdk-aac \) - -[//]: # ( --enable-libfreetype \) - -[//]: # ( --enable-libmp3lame \) - -[//]: # ( --enable-libopus \) - -[//]: # ( --enable-libvorbis \) - -[//]: # ( --enable-libvpx \) - -[//]: # ( --enable-libx264 \) - -[//]: # ( --enable-libx265 \) - -[//]: # ( --enable-pic \) - -[//]: # ( --enable-shared \) - -[//]: # ( --enable-nonfree \) - -[//]: # () -[//]: # (# build and install) - -[//]: # (make -j4) - -[//]: # (sudo make install) - -[//]: # (```) - -[//]: # () -[//]: # () -[//]: # (### 5. librealsense2 2.55.1 install) - -[//]: # (```shell) - -[//]: # (sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE) - -[//]: # (sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u) - -[//]: # (sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y) - -[//]: # (sudo apt-get install \) - -[//]: # (librealsense2=2.55.1-0~realsense.12473 \) - -[//]: # (librealsense2-utils=2.55.1-0~realsense.12473 \) - -[//]: # (librealsense2-dev=2.55.1-0~realsense.12473 \) - -[//]: # (librealsense2-dbg=2.55.1-0~realsense.12473 -y) - -[//]: # (```) - -[//]: # () -[//]: # (### 5. OSQPEigen install(build from source)) - -[//]: # (#### 5.1 install osqp (v0.6.3)) - -[//]: # (```shell) - -[//]: # (git clone --branch v0.6.3 https://github.com/osqp/osqp.git) - -[//]: # (cd osqp) - -[//]: # (git submodule init) - -[//]: # (git submodule update) - -[//]: # (mkdir build) - -[//]: # (cd build) - -[//]: # (cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..) - -[//]: # (make -j) - -[//]: # (sudo make install) - -[//]: # (```) - -[//]: # (#### 5.2 install OsqpEigen (v0.10.1)) - -[//]: # (```shell) - -[//]: # (git clone --branch v0.10.1 https://github.com/robotology/osqp-eigen.git) - -[//]: # (cd osqp-eigen) - -[//]: # (git submodule init) - -[//]: # (git submodule update) - -[//]: # (mkdir build) - -[//]: # (cd build) - -[//]: # (cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..) - -[//]: # (make -j) - -[//]: # (sudo make install) - -[//]: # (```) - -[//]: # () -[//]: # (#### 6 mech-eye-sdk) - -[//]: # (```shell) - -[//]: # (cd assets) - -[//]: # (sudo dkpg -i Mech-Eye_API_2.5.1_amd64.deb) - -[//]: # (```) - -[//]: # () -[//]: # (### 串口冲突问题) - -[//]: # (```shell) - -[//]: # (sudo systemctl stop brltty) - -[//]: # (sudo systemctl disable brltty) - -[//]: # (sudo apt remove brltty) - -[//]: # (```) - -[//]: # () -[//]: # (### ~./bashrc) - -[//]: # (```shell) - -[//]: # (export ARCH="x86") - -[//]: # (export CMVR_ES_PROJECT_PATH="/home/cmvr/Projects/cmvr-es") - -[//]: # (# >>> cmake ) - -[//]: # (export PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/cmake/v3.30.3/cmake-3.30.3-linux-x86_64/bin:$PATH") - -[//]: # (# <<< cmake) - -[//]: # () -[//]: # (# >>> ffmpeg) - -[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/x264/v165/lib:$LD_LIBRARY_PATH") - -[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/x265/v215/lib:$LD_LIBRARY_PATH") - -[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/ffmpeg/v4.2/lib:$LD_LIBRARY_PATH") - -[//]: # (# <<< ffmpeg) - -[//]: # (```) - -[//]: # () +# realsense +sudo apt-get install -y \ + libusb-1.0-0-dev libudev-dev \ + libglu1-mesa-dev +``` diff --git a/cmvr-es/common/utils/image/image_process.h b/cmvr-es/common/utils/image/image_process.h index 03e9dce4..cc8a280a 100644 --- a/cmvr-es/common/utils/image/image_process.h +++ b/cmvr-es/common/utils/image/image_process.h @@ -6,55 +6,284 @@ #include #include +#include +#include #include +#include +#include "devices/camera/abstract_camera.h" #include namespace cmvr { -// 在像素 (u,v) 的邻域内采样深度并取中值，输出单位米。 -// 支持: -// - CV_32FC1: 米 -// - CV_16UC1: 毫米（内部转换为米） -inline bool sampleDepthMeters(const cv::Mat& depth, - int u, - int v, - double& z_m, - int kernel_half = 1, - double min_valid_m = 1e-4) { - if (depth.empty() || depth.channels() != 1) return false; - if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false; - if (kernel_half < 0) return false; - - std::vector valid; - valid.reserve(static_cast((2 * kernel_half + 1) * (2 * kernel_half + 1))); - - for (int dv = -kernel_half; dv <= kernel_half; ++dv) { - for (int du = -kernel_half; du <= kernel_half; ++du) { - const int uu = u + du; - const int vv = v + dv; - if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue; - - double z = 0.0; - if (depth.type() == CV_32FC1) { - z = static_cast(depth.at(vv, uu)); - } else if (depth.type() == CV_16UC1) { - z = static_cast(depth.at(vv, uu)) * 1e-3; - } else { - return false; - } - - if (std::isfinite(z) && z > min_valid_m) { - valid.push_back(z); - } +/** + * @brief 图像几何处理静态工具类。 + * + * 提供像素射线反投影、3D 点投影和深度稳健采样等通用能力。 + */ +class ImageProcess final { + public: + /** + * @brief 将像素坐标反投影为相机坐标系下单位射线（无畸变 pinhole）。 + * + * @param intrinsics 相机内参。 + * @param u 像素横坐标（列）。 + * @param v 像素纵坐标（行）。 + * @param ray_c 输出单位射线方向（相机坐标系）。 + * @return 成功返回 `true`，失败返回 `false`。 + */ + static inline bool pixelToRayCamera(const device::Rs2Intrinsics& intrinsics, + int u, + int v, + Eigen::Vector3d& ray_c) { + const double fx = static_cast(intrinsics.fx); + const double fy = static_cast(intrinsics.fy); + const double cx = static_cast(intrinsics.cx); + const double cy = static_cast(intrinsics.cy); + if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 || + !std::isfinite(cx) || !std::isfinite(cy)) { + return false; } + + ray_c.z() = 1.0; + ray_c.x() = (static_cast(u) - cx) / fx; + ray_c.y() = (static_cast(v) - cy) / fy; + const double n = ray_c.norm(); + if (!std::isfinite(n) || n <= 1e-12) { + return false; + } + ray_c /= n; + return true; } - if (valid.empty()) return false; - std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end()); - z_m = valid[valid.size() / 2]; - return true; -} + /** + * @brief 将相机坐标系下 3D 点投影到像素坐标（无畸变 pinhole）。 + * + * @param intrinsics 相机内参。 + * @param p_c 输入 3D 点（相机坐标系，单位米）。 + * @param uv 输出像素坐标。 + * @return 成功返回 `true`，失败返回 `false`。 + */ + static inline bool projectCameraPointToPixel(const device::Rs2Intrinsics& intrinsics, + const Eigen::Vector3d& p_c, + Eigen::Vector2d& uv) { + if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z()) || + p_c.z() <= 1e-9) { + return false; + } + + const double fx = static_cast(intrinsics.fx); + const double fy = static_cast(intrinsics.fy); + const double cx = static_cast(intrinsics.cx); + const double cy = static_cast(intrinsics.cy); + if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 || + !std::isfinite(cx) || !std::isfinite(cy)) { + return false; + } + + uv.x() = fx * (p_c.x() / p_c.z()) + cx; + uv.y() = fy * (p_c.y() / p_c.z()) + cy; + return std::isfinite(uv.x()) && std::isfinite(uv.y()); + } + + /** + * @brief 将像素坐标和深度值反投影为相机坐标系下 3D 点。 + * + * @param intrinsics 相机内参。 + * @param u 像素横坐标（列）。 + * @param v 像素纵坐标（行）。 + * @param z_m 深度值（米）。 + * @param p_c 输出 3D 点（相机坐标系，单位米）。 + * @return 成功返回 `true`，失败返回 `false`。 + */ + static inline bool pixelToCameraPoint(const device::Rs2Intrinsics& intrinsics, + int u, + int v, + double z_m, + Eigen::Vector3d& p_c) { + const double fx = static_cast(intrinsics.fx); + const double fy = static_cast(intrinsics.fy); + const double cx = static_cast(intrinsics.cx); + const double cy = static_cast(intrinsics.cy); + if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 || + !std::isfinite(cx) || !std::isfinite(cy)) { + return false; + } + if (!std::isfinite(z_m) || z_m <= 1e-9) { + return false; + } + + p_c.z() = z_m; + p_c.x() = (static_cast(u) - cx) / fx * z_m; + p_c.y() = (static_cast(v) - cy) / fy * z_m; + return std::isfinite(p_c.x()) && std::isfinite(p_c.y()) && std::isfinite(p_c.z()); + } + + /** + * @brief 在像素邻域采样深度并取中值，支持 MAD 离群剔除，输出单位米。 + * + * 支持深度图类型： + * - `CV_16UC1`：毫米（内部转换为米） + * - `CV_32FC1`：米（若值明显为毫米尺度会自动转米） + * - `CV_64FC1`：米（若值明显为毫米尺度会自动转米） + * + * @param depth 深度图。 + * @param u 像素横坐标（列）。 + * @param v 像素纵坐标（行）。 + * @param z_m 输出深度值（米）。 + * @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。 + * @param enable_outlier_reject 是否启用离群值剔除。 + * @param outlier_sigma MAD 门限倍数。 + * @param outlier_min_dev_m 最小离群门限（米）。 + * @param min_valid_m 最小有效深度（米）。 + * @return 成功返回 `true`，失败返回 `false`。 + */ + static inline bool sampleDepthMeters(const cv::Mat& depth, + int u, + int v, + double& z_m, + int window_size_px = 3, + bool enable_outlier_reject = true, + double outlier_sigma = 2.5, + double outlier_min_dev_m = 0.002, + double min_valid_m = 1e-4) { + if (depth.empty() || depth.channels() != 1) { + return false; + } + if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) { + return false; + } + + const int type = depth.type(); + if (type != CV_16UC1 && type != CV_32FC1 && type != CV_64FC1) { + return false; + } + + auto medianOf = [](std::vector values, double& m) -> bool { + if (values.empty()) { + return false; + } + const size_t k = values.size() / 2; + std::nth_element(values.begin(), values.begin() + k, values.end()); + m = values[k]; + return std::isfinite(m); + }; + + int w = std::max(1, window_size_px); + if ((w % 2) == 0) { + ++w; + } + const int radius = w / 2; + + std::vector valid_depths; + valid_depths.reserve(static_cast((2 * radius + 1) * (2 * radius + 1))); + for (int yy = std::max(0, v - radius); yy <= std::min(depth.rows - 1, v + radius); ++yy) { + for (int xx = std::max(0, u - radius); xx <= std::min(depth.cols - 1, u + radius); ++xx) { + double z = std::numeric_limits::quiet_NaN(); + if (type == CV_16UC1) { + const uint16_t raw = depth.at(yy, xx); + if (raw == 0) { + continue; + } + z = static_cast(raw) * 1e-3; // mm -> m + } else if (type == CV_32FC1) { + z = static_cast(depth.at(yy, xx)); + } else { // CV_64FC1 + z = depth.at(yy, xx); + } + + if (std::isfinite(z) && z > 100.0) { + z *= 1e-3; // 兼容浮点毫米 + } + if (!std::isfinite(z) || z <= min_valid_m || z >= 50.0) { + continue; + } + valid_depths.push_back(z); + } + } + + if (valid_depths.empty()) { + return false; + } + + double med = 0.0; + if (!medianOf(valid_depths, med)) { + return false; + } + + if (enable_outlier_reject && std::isfinite(outlier_sigma) && outlier_sigma > 0.0) { + std::vector abs_dev; + abs_dev.reserve(valid_depths.size()); + for (const double z : valid_depths) { + abs_dev.push_back(std::abs(z - med)); + } + double mad = 0.0; + if (medianOf(abs_dev, mad)) { + const double robust_sigma = 1.4826 * mad; + const double gate = std::max(std::max(0.0, outlier_min_dev_m), outlier_sigma * robust_sigma); + std::vector inliers; + inliers.reserve(valid_depths.size()); + for (const double z : valid_depths) { + if (std::abs(z - med) <= gate) { + inliers.push_back(z); + } + } + if (!inliers.empty() && medianOf(inliers, med)) { + // use filtered median + } + } + } + + z_m = med; + return std::isfinite(z_m) && z_m > 0.0; + } + + /** + * @brief 从深度图直接将像素反投影为相机坐标系下 3D 点。 + * + * 该函数先调用 `sampleDepthMeters(...)` 估计像素深度，再调用 + * `pixelToCameraPoint(...)` 完成反投影。 + * + * @param intrinsics 相机内参。 + * @param depth 深度图。 + * @param u 像素横坐标（列）。 + * @param v 像素纵坐标（行）。 + * @param p_c 输出 3D 点（相机坐标系，单位米）。 + * @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。 + * @param enable_outlier_reject 是否启用离群值剔除。 + * @param outlier_sigma MAD 门限倍数。 + * @param outlier_min_dev_m 最小离群门限（米）。 + * @param min_valid_m 最小有效深度（米）。 + * @return 成功返回 `true`，失败返回 `false`。 + */ + static inline bool pixelToCameraPointWithDepth(const device::Rs2Intrinsics& intrinsics, + const cv::Mat& depth, + int u, + int v, + Eigen::Vector3d& p_c, + int window_size_px = 3, + bool enable_outlier_reject = true, + double outlier_sigma = 2.5, + double outlier_min_dev_m = 0.002, + double min_valid_m = 1e-4) { + double z_m = 0.0; + if (!sampleDepthMeters(depth, + u, + v, + z_m, + window_size_px, + enable_outlier_reject, + outlier_sigma, + outlier_min_dev_m, + min_valid_m)) { + return false; + } + return pixelToCameraPoint(intrinsics, u, v, z_m, p_c); + } + + private: + /** @brief 禁止实例化。 */ + ImageProcess() = delete; +}; } // namespace cmvr - diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index 4657707b..dcaa3eec 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -303,6 +303,7 @@ private: Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()}; // 坐标对齐 + // R = R_camera_urdf_ * （R_cv_转置） == ViSP相机坐标系 到 URDF 相机系 的等效旋转 // AbstractCamera -> ViSP 的旋转矩阵。 Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()}; // AbstractCamera -> URDF 相机系的旋转矩阵。 diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index 4943d0ac..5f5adaee 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -248,7 +248,7 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, const int u = static_cast(std::lround(fx * x_depth_ctrl + cx)); const int v = static_cast(std::lround(fy * y_depth_ctrl + cy)); double z_from_depth = 0.0; - if (sampleDepthMeters(depth, u, v, z_from_depth)) { + 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) { diff --git a/cmvr-es/perception/CMakeLists.txt b/cmvr-es/perception/CMakeLists.txt index 322ca2d1..6e513c84 100644 --- a/cmvr-es/perception/CMakeLists.txt +++ b/cmvr-es/perception/CMakeLists.txt @@ -31,17 +31,17 @@ target_link_libraries(perception PUBLIC add_library(cmvr_es::perception ALIAS perception) install(TARGETS perception LIBRARY DESTINATION lib) -#add_executable(tag_relative_target_3d_test -# src/tag_relative_target_3d_test.cpp -#) -# -#target_link_libraries(tag_relative_target_3d_test -# PRIVATE -# cmvr_es::perception -# cmvr_es::device::realsense_camera -# cmvr_es::proto -# glog -# gtest -# gtest_main -# pthread -#) +add_executable(tag_relative_target_3d_test + src/tag_relative_target_3d_test.cpp +) + +target_link_libraries(tag_relative_target_3d_test + PRIVATE + cmvr_es::perception + cmvr_es::device::realsense_camera + cmvr_es::proto + glog + gtest + gtest_main + pthread +) diff --git a/cmvr-es/perception/include/tag_relative_target_3d.h b/cmvr-es/perception/include/tag_relative_target_3d.h index 79158361..37fe0434 100644 --- a/cmvr-es/perception/include/tag_relative_target_3d.h +++ b/cmvr-es/perception/include/tag_relative_target_3d.h @@ -1,215 +1,457 @@ -// -// Created by lgv on 2026/2/26. -// - #pragma once +#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H +#define CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H #include #include #include #include +#include #include + #include "devices/camera/abstract_camera.h" namespace cmvr::perception { /** - * @brief 单个 tag 观测：tag 相对于相机的位姿（`T_c_t`）。 - */ -struct TagPoseObservation { - // tag id。 - int tag_id{-1}; - // tag 坐标系 -> 相机坐标系 变换。 - Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; - // 观测权重（<=0 或非法时按 1.0 处理）。 - double weight{1.0}; -}; - -/** - * @brief 维护“目标点在各 tag 坐标系下的 3D 相对坐标”，并支持在线恢复目标点相机坐标。 + * @brief Tag 相对目标 3D 跟踪器（全封装版）。 * - * 典型流程： - * 1) 锁定目标时已知目标点相机坐标 `p_c_target` 与可见 tag 位姿观测； - * 2) 计算并缓存每个可见 tag 下的 `p_ti_target`； - * 3) 运行时只要某个已缓存的 tag 可见，就可恢复目标点在当前相机系下位置。 + * 该类内部完成： + * 1) 从相机抓取 RGB 或 RGBD； + * 2) AprilTag 检测与位姿估计； + * 3) 目标点像素 `(u, v)` 到相机 3D 点的求解； + * 4) 目标点在 tag 坐标系下的锚点缓存与基于 active tag 的跟踪。 + * + * 对外仅暴露 `(u, v)` 输入，结果通过 `last*` getter 读取。 */ class TagRelativeTarget3D { public: /** - * @brief 最近一次调用状态。 + * @brief 最近一次调用的状态码。 */ enum class Status { - OK = 0, // 计算成功 - INVALID_INPUT, // 输入参数非法 - NO_CAMERA, // 未注入相机 - NO_NEW_FRAME, // 相机未取到新图像 - BAD_IMAGE, // 图像或内参非法 - NO_TAG, // 未检测到 tag - NO_OBSERVATION, // 无可用观测 - TARGET_NOT_LOCKED, // 尚未缓存任何 tag 锚点 - NO_MATCHING_TAG // 有观测，但没有命中已缓存的 tag + OK = 0, ///< 调用成功。 + INVALID_INPUT, ///< 输入参数非法。 + NO_CAMERA, ///< 相机对象为空。 + NO_NEW_FRAME, ///< 未获取到新图像帧。 + BAD_IMAGE, ///< 图像或内参无效。 + NO_TAG, ///< 未检测到 tag。 + NO_OBSERVATION, ///< 当前无可用 tag 观测。 + TARGET_NOT_LOCKED, ///< 尚未建立目标锚点。 + NO_MATCHING_TAG ///< 有观测但没有命中已缓存锚点。 }; /** - * @brief 构造函数，支持直接注入抽象相机。 - * @param camera 抽象相机，可为空（后续可仅使用几何接口）。 + * @brief 目标像素转 3D 点的方法。 + */ + enum class TargetPointMethod { + TAG_PLANE = 0, ///< 像素射线与 tag 平面求交（多 tag 融合）。 + DEPTH_IMAGE ///< 深度反投影（要求 RGBD 对齐）。 + }; + + /** + * @brief 构造函数。 + * @param camera 抽象相机对象，可为空。 */ explicit TagRelativeTarget3D(const std::shared_ptr& camera = nullptr); + ~TagRelativeTarget3D() = default; /** * @brief 设置/替换相机对象。 - * @param camera 抽象相机。 + * @param camera 抽象相机对象。 */ - void setCamera(const std::shared_ptr& camera) { camera_ = camera; } + void setCamera(const std::shared_ptr& camera); /** - * @brief 清空全部 tag 锚点。 + * @brief 清空内部状态（锚点、缓存、追踪状态与最近输出）。 */ void clear(); /** - * @brief 是否没有任何缓存锚点。 - * @return 为空返回 `true`。 + * @brief 是否没有任何已缓存锚点。 + * @return 没有锚点返回 `true`。 */ bool empty() const { return anchors_in_tag_.empty(); } /** - * @brief 当前缓存锚点数量。 + * @brief 当前已缓存锚点数量。 * @return 锚点数量。 */ size_t anchorCount() const { return anchors_in_tag_.size(); } /** - * @brief 获取最近状态。 - * @return 最近状态枚举。 + * @brief 获取最近一次调用状态。 + * @return 状态码。 */ Status lastStatus() const { return last_status_; } /** - * @brief 状态枚举转字符串。 - * @param status 状态枚举。 - * @return 字符串常量。 + * @brief 状态码转可读字符串。 + * @param status 状态码。 + * @return 对应字符串常量。 */ static const char* statusToString(Status status); /** - * @brief 设置 AprilTag 边长。 - * @param tag_size_m tag 边长（米）。 + * @brief 设置目标像素转 3D 方法。 + * @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 通过内部相机抓帧并检测 tag 观测（`T_c_t`）。 - * @param observations 输出观测列表。 - * @return 检测成功返回 `true`。 + * @brief 设置深度采样与离群剔除参数。 + * + * 该配置仅在 `DEPTH_IMAGE` 模式生效。 + * + * @param window_size_px 深度采样窗口边长（像素，自动修正为奇数）。 + * @param enable_outlier_reject 是否启用离群值剔除。 + * @param outlier_sigma MAD 门限倍数。 + * @param outlier_min_dev_m 离群剔除最小绝对阈值（米）。 */ - bool detectObservations(std::vector& observations); + void setDepthSamplingConfig(int window_size_px = 3, + bool enable_outlier_reject = true, + double outlier_sigma = 2.5, + double outlier_min_dev_m = 0.003); /** - * @brief 使用外部图像与内参检测 tag 观测（`T_c_t`）。 - * @param color 输入彩色/灰度图。 - * @param intrinsics 相机内参。 - * @param observations 输出观测列表。 - * @return 检测成功返回 `true`。 + * @brief 设置 active tag 切换策略。 + * @param missing_before_switch active tag 连续丢失多少帧后允许切换。 + * @param switch_hysteresis 新候选切换阈值倍率（>1 时更保守）。 */ - bool detectObservationsFromImage(const cv::Mat& color, - const device::Rs2Intrinsics& intrinsics, - std::vector& observations); + void setActiveTagSwitchPolicy(int missing_before_switch = 4, + double switch_hysteresis = 1.2); /** - * @brief 使用内部相机检测结果直接建立/更新锚点。 - * @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。 - * @param overwrite_existing 是否覆盖已存在锚点。 - * @return 成功建立至少一个锚点返回 `true`。 + * @brief 设置 active tag 候选评分权重。 + * @param weight_power 观测权重项指数。 + * @param proximity_power 距离项指数（目标越靠近 tag 原点评分越高）。 + * @param proximity_scale_m 距离归一化尺度（米）。 */ - bool lockTargetInCamera(const Eigen::Vector3d& p_c_target, - bool overwrite_existing = true); + void setTrackingCandidateScoreWeights(double weight_power = 1.0, + double proximity_power = 1.0, + double proximity_scale_m = 0.08); /** - * @brief 使用内部相机检测结果直接恢复目标点相机坐标。 - * @param p_c_target_out 输出目标点相机坐标（米）。 - * @param used_tag_id_out 若非空，输出主导 tag id。 - * @param spread_out 若非空，输出多 tag 融合离散误差（米）。 - * @return 恢复成功返回 `true`。 + * @brief 重置 active tag 跟踪状态（不清空锚点）。 */ - bool resolveTargetInCamera(Eigen::Vector3d& p_c_target_out, - int* used_tag_id_out = nullptr, - double* spread_out = nullptr); + void resetActiveTagTracking(); /** - * @brief 根据“目标点相机坐标 + 可见 tag 观测”批量建立/更新各 tag 锚点。 - * @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。 - * @param observations 可见 tag 观测集合。 - * @param overwrite_existing 是否覆盖已存在的同 id 锚点。 - * @return 成功建立至少一个锚点时返回 `true`。 + * @brief 当前 active tag id。 + * @return active tag id；无效时为 `-1`。 */ - bool lockTargetInCamera(const Eigen::Vector3d& p_c_target, - const std::vector& observations, - bool overwrite_existing = true); + int activeTagId() const { return active_tag_id_; } /** - * @brief 用单个 tag 观测建立/更新锚点。 + * @brief 当前 active tag 连续丢失帧数。 + * @return 丢失帧计数。 + */ + int activeTagMissingCount() const { return active_tag_missing_count_; } + + + /** + * @brief 对目标像素进行单次 3D 解算。 + * @param u 像素列坐标。 + * @param v 像素行坐标。 + * @return 成功返回 `true`，结果写入 `last*`。 + */ + bool solveFromPixel(int u, int v); + + /** + * @brief 以目标像素初始化跟踪（解算 + 建锚点 + 选择 active tag）。 + * @param u 像素列坐标。 + * @param v 像素行坐标。 + * @return 成功返回 `true`，结果写入 `last*`。 + */ + bool startTrackingFromPixel(int u, int v); + + /** + * @brief 进行一次跟踪迭代（active tag 跟踪 + 必要时自动切换）。 + * @return 成功返回 `true`，结果写入 `last*`。 + */ + bool track(); + + /** + * @brief 最近一次成功解算/跟踪得到的目标点相机坐标。 + * @return 目标点 `p_c_target`（米）。 + */ + const Eigen::Vector3d& lastTargetInCamera() const { return last_p_c_target_; } + + /** + * @brief 最近一次结果对应的主导 tag id。 + * @return 平面法为主导 tag id；深度法通常为 `-1`。 + */ + int lastUsedTagId() const { return last_used_tag_id_; } + + /** + * @brief 最近一次平面法融合离散度。 + * @return 离散度（米）；深度法通常为 0。 + */ + double lastSpread() const { return last_spread_m_; } + + /** + * @brief 最近一次 `track()` 是否发生 active tag 切换。 + * @return 切换返回 `true`。 + */ + bool lastSwitched() const { return last_switched_; } + + /** + * @brief 是否存在指定 tag 的目标锚点。 * @param tag_id tag id。 - * @param T_c_t tag 坐标系到相机坐标系变换。 - * @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。 - * @return 建立成功返回 `true`。 - */ - bool upsertAnchorFromCamera(int tag_id, - const Eigen::Matrix4d& T_c_t, - const Eigen::Vector3d& p_c_target); - - /** - * @brief 根据当前可见 tag，恢复目标点在当前相机坐标系下的 3D 坐标。 - * @param observations 当前可见 tag 观测。 - * @param p_c_target_out 输出目标点相机坐标（米）。 - * @param used_tag_id_out 若非空，输出本次主导 tag id（仅 1 个匹配时等于该 id，多匹配时为最大权重 id）。 - * @param spread_out 若非空，输出多 tag 融合后最大离散误差（米）。 - * @return 恢复成功返回 `true`。 - */ - bool resolveTargetInCamera(const std::vector& observations, - Eigen::Vector3d& p_c_target_out, - int* used_tag_id_out = nullptr, - double* spread_out = nullptr); - - /** - * @brief 是否已缓存指定 tag 的锚点。 - * @param tag_id tag id。 - * @return 命中返回 `true`。 + * @return 存在返回 `true`。 */ bool hasAnchorForTag(int tag_id) const; /** - * @brief 获取目标点在指定 tag 坐标系下的 3D 相对坐标。 + * @brief 获取指定 tag 坐标系下缓存的目标点坐标。 * @param tag_id tag id。 - * @param p_t_target_out 输出 `p_t_target`（米）。 - * @return 获取成功返回 `true`。 + * @param p_t_target_out 输出目标点在该 tag 坐标系下的坐标。 + * @return 成功返回 `true`。 */ bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const; +private: /** - * @brief 获取构造时注入的相机对象。 - * @return 抽象相机智能指针。 + * @brief 单帧 tag 位姿观测（tag 坐标系到相机坐标系）。 */ - const std::shared_ptr& camera() const { return camera_; } + struct TagPoseObservation { + // 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}; + }; private: + /** + * @brief 重置最近一次调用输出缓存。 + */ + void resetLastOutputs(); + + /** + * @brief 内部抓帧与观测更新入口。 + * @param need_depth 是否需要深度图。 + * @param need_tags 是否需要更新 tag 观测。 + * @return 成功返回 `true`。 + */ + bool updateInternal(bool need_depth, bool need_tags); + + /** + * @brief 抓取一帧 RGB 与内参。 + * @return 成功返回 `true`。 + */ + bool grabRGB(); + + /** + * @brief 抓取一帧 RGBD 与内参。 + * @return 成功返回 `true`。 + */ + bool grabRGBD(); + + /** + * @brief 从当前缓存 RGB + 内参中检测 tag 并更新 `observations_`。 + * @return 成功返回 `true`。 + */ + bool updateObservations(); // uses frame_.color + frame_.intr + + /** + * @brief 依据当前观测将目标点写入各 tag 锚点缓存。 + * @param p_c_target 目标点在相机坐标系下位置。 + * @param overwrite_existing 是否覆盖已有锚点。 + * @return 成功返回 `true`。 + */ + bool lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing); + + /** + * @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`。 + */ + bool initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock, + int used_tag, + double spread); + + /** + * @brief 在当前缓存观测下执行 active tag 跟踪。 + * @return 成功返回 `true`。 + */ + bool trackWithActiveTagFromCached(); + + /** + * @brief 判断 4x4 矩阵元素是否全为有限数。 + * @param T 输入矩阵。 + * @return 全有限返回 `true`。 + */ static bool isFiniteMatrix(const Eigen::Matrix4d& T); + + /** + * @brief 判断 3D 向量是否全为有限数。 + * @param p 输入向量。 + * @return 全有限返回 `true`。 + */ static bool isFiniteVector(const Eigen::Vector3d& p); + + /** + * @brief 点从 tag 坐标系变换到相机坐标系。 + * @param T_c_t tag 到相机变换。 + * @param p_t tag 坐标系下点。 + * @return 相机坐标系下点。 + */ 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 坐标系下点。 + */ 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 可选输出：视线与平面法向夹角余弦绝对值。 + * @return 求交成功返回 `true`。 + */ + static bool intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t, + const Eigen::Vector3d& ray_c, + Eigen::Vector3d& p_c_intersection, + double* view_cos_out = nullptr); + + /** + * @brief 归一化观测权重（非法权重回退到默认值）。 + * @param w 输入权重。 + * @return 处理后的权重。 + */ + static double sanitizeObsWeight(double w); + + /** + * @brief 计算单个观测作为 active tag 候选的评分。 + * @param obs tag 观测。 + * @return 候选评分。 + */ + double trackingCandidateScore(const TagPoseObservation& obs) const; + private: - // 缓存: tag_id -> 目标点在该 tag 坐标系下的 3D 坐标。 + // 目标锚点缓存：tag_id -> p_t_target。 std::unordered_map anchors_in_tag_; - // 抽象相机对象（由构造函数注入）。 + + // 相机对象。 std::shared_ptr camera_{nullptr}; + // AprilTag 检测器。 vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11}; - // tag 边长（米）。 + // AprilTag 物理边长（米）。 double tag_size_m_{0.12}; + // 最近一次调用状态。 Status last_status_{Status::TARGET_NOT_LOCKED}; + + // 图像/内参缓存。 + FrameCache frame_; + // 当前帧 tag 观测缓存。 + std::vector observations_; + // 观测缓存对应的帧 id。 + uint64_t obs_frame_id_{0}; + + // 当前 active tag id。 + int active_tag_id_{-1}; + // active tag 连续丢失帧计数。 + int active_tag_missing_count_{0}; + // 连续丢失多少帧后允许切换 active tag。 + int missing_before_switch_{4}; + // active 切换迟滞阈值。 + double switch_hysteresis_{1.2}; + + // 候选评分中权重项指数。 + double candidate_weight_power_{1.0}; + // 候选评分中距离项指数。 + double candidate_proximity_power_{1.0}; + // 候选评分距离归一化尺度（米）。 + 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}; + + // 最近一次输出的目标点相机坐标。 + Eigen::Vector3d last_p_c_target_{Eigen::Vector3d::Zero()}; + // 最近一次输出使用的主导 tag id。 + int last_used_tag_id_{-1}; + // 最近一次输出离散度（米）。 + double last_spread_m_{0.0}; + // 最近一次 track 是否发生 active tag 切换。 + bool last_switched_{false}; }; } // namespace cmvr::perception + +#endif // CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H diff --git a/cmvr-es/perception/src/tag_relative_target_3d.cpp b/cmvr-es/perception/src/tag_relative_target_3d.cpp index ffae4a32..846052fc 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d.cpp +++ b/cmvr-es/perception/src/tag_relative_target_3d.cpp @@ -1,25 +1,82 @@ -// -// Created by lgv on 2026/2/26. -// - #include "perception/include/tag_relative_target_3d.h" #include #include +#include #include #include #include #include +#include #include +#include "common/utils/image/image_process.h" + namespace cmvr::perception { +namespace { + +// -------- helpers -------- +double sanitizeWeight(double w) { + if (!std::isfinite(w) || w <= 0.0) return 1.0; + return w; +} + +class WeightedPointFusion { +public: + void reserve(size_t n) { points_.reserve(n); } + + void addPoint(int tag_id, const Eigen::Vector3d& p_c, double weight) { + if (!std::isfinite(weight) || weight <= 0.0) return; + points_.push_back(p_c); + weighted_sum_ += weight * p_c; + weight_sum_ += weight; + if (weight > best_weight_) { + best_weight_ = weight; + best_id_ = tag_id; + } + } + + 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()); + } + spread_out = max_err; + return true; + } + +private: + std::vector points_; + Eigen::Vector3d weighted_sum_ = Eigen::Vector3d::Zero(); + double weight_sum_ = 0.0; + double best_weight_ = -std::numeric_limits::infinity(); + int best_id_ = -1; +}; + +} // namespace TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr& camera) : camera_(camera) { detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); } +void TagRelativeTarget3D::setCamera(const std::shared_ptr& camera) { + camera_ = camera; + frame_ = FrameCache{}; + observations_.clear(); + obs_frame_id_ = 0; + anchors_in_tag_.clear(); + resetActiveTagTracking(); + resetLastOutputs(); + last_status_ = Status::TARGET_NOT_LOCKED; +} + const char* TagRelativeTarget3D::statusToString(Status status) { switch (status) { case Status::OK: return "ok"; @@ -35,82 +92,303 @@ 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; + 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, + double outlier_min_dev_m) { + int w = std::max(1, window_size_px); + if ((w % 2) == 0) ++w; + w = std::min(w, 31); + 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; +} + +void TagRelativeTarget3D::setActiveTagSwitchPolicy(int missing_before_switch, + double switch_hysteresis) { + missing_before_switch_ = std::max(1, missing_before_switch); + if (std::isfinite(switch_hysteresis) && switch_hysteresis > 0.0) { + switch_hysteresis_ = std::max(1.0, switch_hysteresis); } } -bool TagRelativeTarget3D::detectObservations(std::vector& observations) { - observations.clear(); +void TagRelativeTarget3D::setTrackingCandidateScoreWeights(double weight_power, + double proximity_power, + double proximity_scale_m) { + if (std::isfinite(weight_power) && weight_power >= 0.0) candidate_weight_power_ = weight_power; + if (std::isfinite(proximity_power) && proximity_power >= 0.0) candidate_proximity_power_ = proximity_power; + if (std::isfinite(proximity_scale_m) && proximity_scale_m > 1e-6) candidate_proximity_scale_m_ = proximity_scale_m; +} +void TagRelativeTarget3D::resetActiveTagTracking() { + active_tag_id_ = -1; + active_tag_missing_count_ = 0; +} + +// ========================= +// Internal frame grabbing +// ========================= +bool TagRelativeTarget3D::grabRGB() { if (!camera_) { last_status_ = Status::NO_CAMERA; return false; } - cv::Mat color; - device::Rs2Intrinsics intrinsics{}; + frame_.has_color = frame_.has_depth = frame_.has_intr = false; + try { - // Tag 检测只需要彩色图像与相机内参，避免依赖深度流导致阻塞/超时。 - camera_->getRGBImage(color, intrinsics); - } catch (const std::exception&) { + 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++; + return true; +} + +bool TagRelativeTarget3D::grabRGBD() { + if (!camera_) { + last_status_ = Status::NO_CAMERA; + 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; + return false; + } + + if (frame_.color.empty() || frame_.depth.empty()) { + last_status_ = Status::NO_NEW_FRAME; + return false; + } + + 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 (need_tags) { + if (!updateObservations()) return false; + } else { + last_status_ = Status::OK; + } + 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; } - - return detectObservationsFromImage(color, intrinsics, observations); -} - -bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target, - bool overwrite_existing) { - std::vector observations; - if (!detectObservations(observations)) { + if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) { + last_status_ = Status::BAD_IMAGE; return false; } - return lockTargetInCamera(p_c_target, observations, overwrite_existing); -} -bool TagRelativeTarget3D::resolveTargetInCamera(Eigen::Vector3d& p_c_target_out, - int* used_tag_id_out, - double* spread_out) { - std::vector observations; - if (!detectObservations(observations)) { + const double fx = static_cast(frame_.intr.fx); + const double fy = static_cast(frame_.intr.fy); + const double cx = static_cast(frame_.intr.cx); + const double cy = static_cast(frame_.intr.cy); + if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) { + last_status_ = Status::BAD_IMAGE; return false; } - return resolveTargetInCamera(observations, p_c_target_out, used_tag_id_out, spread_out); + + cv::Mat gray; + if (color.channels() == 1) gray = color; + else if (color.channels() == 3) cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY); + else cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY); + + if (gray.empty() || gray.type() != CV_8UC1) { + last_status_ = Status::BAD_IMAGE; + return false; + } + if (!gray.isContinuous()) gray = gray.clone(); + + const int width = gray.cols; + const int height = gray.rows; + + vpCameraParameters cam; + cam.initPersProjWithoutDistortion(fx, fy, cx, cy); + + vpImage I(height, width); + for (int y = 0; y < height; ++y) { + std::memcpy(I[y], gray.ptr(y), static_cast(width)); + } + + std::vector cMo_vec; + const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec); + if (!detected || cMo_vec.empty()) { + last_status_ = Status::NO_TAG; + obs_frame_id_ = frame_.frame_id; + return false; + } + + const std::vector tag_ids = detector_.getTagsId(); + const std::vector margins = detector_.getTagsDecisionMargin(); + const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size()); + if (pair_size == 0) { + last_status_ = Status::NO_TAG; + obs_frame_id_ = frame_.frame_id; + return false; + } + + observations_.clear(); + observations_.reserve(pair_size); + + for (size_t i = 0; i < pair_size; ++i) { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + const vpHomogeneousMatrix& cMo = cMo_vec[i]; + + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) T(r, c) = cMo[r][c]; + } + T(0, 3) = cMo[0][3]; + T(1, 3) = cMo[1][3]; + T(2, 3) = cMo[2][3]; + + double w = 1.0; + if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) { + w = static_cast(margins[i]); + } + + observations_.push_back(TagPoseObservation{tag_ids[i], T, w}); + } + + if (observations_.empty()) { + last_status_ = Status::NO_TAG; + obs_frame_id_ = frame_.frame_id; + return false; + } + + obs_frame_id_ = frame_.frame_id; + last_status_ = Status::OK; + return true; } -void TagRelativeTarget3D::clear() { - anchors_in_tag_.clear(); - last_status_ = Status::TARGET_NOT_LOCKED; +// ========================= +// 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 (!updateInternal(false, true)) return false; // RGB + tags + return solveFromPixelOnPlaneCached(u, v); } -bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target, - const std::vector& observations, - bool overwrite_existing) { +bool TagRelativeTarget3D::startTrackingFromPixel(int u, int v) { + resetLastOutputs(); + resetActiveTagTracking(); + + const bool need_depth = (target_point_method_ == TargetPointMethod::DEPTH_IMAGE); + if (!updateInternal(need_depth, true)) return false; // 追踪必须要 tags + + // 先求 lock 点（写入 last_*） + if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) { + if (!solveFromPixelWithDepthCached(u, v)) return false; + // 深度法：used_tag=-1, spread=0（已经在 solveFromPixelWithDepthCached 里写好） + } else { + if (!solveFromPixelOnPlaneCached(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); +} + +bool TagRelativeTarget3D::track() { + resetLastOutputs(); + + if (anchors_in_tag_.empty()) { + last_status_ = Status::TARGET_NOT_LOCKED; + return false; + } + + if (!updateInternal(false, true)) return false; // RGB + tags + return trackWithActiveTagFromCached(); +} + +// ========================= +// 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()) { + 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; - } + 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; } @@ -124,120 +402,304 @@ bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target, return true; } -bool TagRelativeTarget3D::upsertAnchorFromCamera(int tag_id, - const Eigen::Matrix4d& T_c_t, - const Eigen::Vector3d& p_c_target) { - if (tag_id < 0 || !isFiniteMatrix(T_c_t) || !isFiniteVector(p_c_target)) { - last_status_ = Status::INVALID_INPUT; - return false; - } - - anchors_in_tag_[tag_id] = pointCameraToTag(T_c_t, p_c_target); - last_status_ = Status::OK; - return true; -} - -bool TagRelativeTarget3D::resolveTargetInCamera(const std::vector& observations, - Eigen::Vector3d& p_c_target_out, - int* used_tag_id_out, - double* spread_out) { - if (used_tag_id_out) { - *used_tag_id_out = -1; - } - if (spread_out) { - *spread_out = 0.0; - } - +bool TagRelativeTarget3D::resolveTargetInCameraInternal() { if (anchors_in_tag_.empty()) { last_status_ = Status::TARGET_NOT_LOCKED; return false; } - if (observations.empty()) { + if (observations_.empty()) { last_status_ = Status::NO_OBSERVATION; return false; } - std::vector matched_points; - matched_points.reserve(observations.size()); + WeightedPointFusion fusion; + fusion.reserve(observations_.size()); - Eigen::Vector3d weighted_sum = Eigen::Vector3d::Zero(); - double weight_sum = 0.0; - double best_weight = -std::numeric_limits::infinity(); - int best_id = -1; - - for (const auto& obs : observations) { - if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) { - continue; - } + 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; - } + if (it == anchors_in_tag_.end()) continue; const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second); - if (!isFiniteVector(p_c)) { - continue; - } + if (!isFiniteVector(p_c)) continue; - double w = obs.weight; - if (!std::isfinite(w) || w <= 0.0) { - w = 1.0; - } - - matched_points.push_back(p_c); - weighted_sum += w * p_c; - weight_sum += w; - - if (w > best_weight) { - best_weight = w; - best_id = obs.tag_id; - } + fusion.addPoint(obs.tag_id, p_c, sanitizeObsWeight(obs.weight)); } - if (matched_points.empty() || weight_sum <= 0.0) { + 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; } - p_c_target_out = weighted_sum / weight_sum; - - if (spread_out) { - double max_err = 0.0; - for (const auto& p : matched_points) { - max_err = std::max(max_err, (p - p_c_target_out).norm()); - } - *spread_out = max_err; - } - - if (used_tag_id_out) { - *used_tag_id_out = best_id; - } - + last_p_c_target_ = p_out; + last_used_tag_id_ = used; + last_spread_m_ = spread; last_status_ = Status::OK; return true; } +// ========================= +// Internal solvers (cached) +// ========================= +bool TagRelativeTarget3D::solveFromPixelOnPlaneCached(int u, int v) { + if (observations_.empty()) { + last_status_ = Status::NO_OBSERVATION; + return false; + } + + Eigen::Vector3d ray_c = Eigen::Vector3d::Zero(); + if (!cmvr::ImageProcess::pixelToRayCamera(frame_.intr, u, v, ray_c)) { + last_status_ = Status::INVALID_INPUT; + return false; + } + + constexpr double kMinViewCos = 0.10; // 掠射剔除 + constexpr double kDistNormPx = 200.0; // 像素邻近性尺度 + + const Eigen::Vector2d uv_target(static_cast(u), static_cast(v)); + WeightedPointFusion fusion; + fusion.reserve(observations_.size()); + + for (const auto& obs : observations_) { + if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue; + + Eigen::Vector3d p_c_intersection = Eigen::Vector3d::Zero(); + double view_cos = 0.0; + 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 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)) { + dist_px = (uv_tag - uv_target).norm(); + } + const double dist_gain = 1.0 / (1.0 + dist_px / kDistNormPx); + + const double score = w_obs * view_cos * dist_gain; + if (!std::isfinite(score) || score <= 0.0) continue; + + fusion.addPoint(obs.tag_id, p_c_intersection, score); + } + + 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; +} + +bool TagRelativeTarget3D::solveFromPixelWithDepthCached(int u, int v) { + if (!frame_.has_depth || frame_.depth.empty() || !frame_.has_intr) { + last_status_ = Status::BAD_IMAGE; + return false; + } + + Eigen::Vector3d p = Eigen::Vector3d::Zero(); + if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(frame_.intr, + frame_.depth, + u, + v, + p, + depth_window_size_px_, + depth_outlier_reject_enabled_, + depth_outlier_sigma_, + depth_outlier_min_dev_m_)) { + last_status_ = Status::BAD_IMAGE; + return false; + } + + last_p_c_target_ = p; + last_used_tag_id_ = -1; + last_spread_m_ = 0.0; + last_status_ = Status::OK; + return true; +} + +bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock, + int used_tag, + double spread) { + if (!isFiniteVector(p_c_lock)) { + last_status_ = Status::INVALID_INPUT; + return false; + } + if (observations_.empty()) { + last_status_ = Status::NO_OBSERVATION; + return false; + } + + // 建锚点（覆盖） + if (!lockTargetInCameraInternal(p_c_lock, true)) return false; + + // 选 active tag + int active_tag = used_tag; + if (active_tag < 0 || !hasAnchorForTag(active_tag)) { + double best_score = -std::numeric_limits::infinity(); + for (const auto& obs : observations_) { + const double sc = trackingCandidateScore(obs); + if (sc > best_score) { + best_score = sc; + active_tag = obs.tag_id; + } + } + } + + if (active_tag < 0 || !hasAnchorForTag(active_tag)) { + last_status_ = Status::NO_MATCHING_TAG; + return false; + } + + 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; + last_switched_ = false; + last_status_ = Status::OK; + return true; +} + +bool TagRelativeTarget3D::trackWithActiveTagFromCached() { + if (observations_.empty()) { + last_status_ = Status::NO_OBSERVATION; + return false; + } + + int active_obs_index = -1; + double active_score = -std::numeric_limits::infinity(); + + int best_candidate_tag = -1; + int best_candidate_index = -1; + double best_candidate_score = -std::numeric_limits::infinity(); + + for (size_t i = 0; i < observations_.size(); ++i) { + const auto& obs = observations_[i]; + const double score = trackingCandidateScore(obs); + if (!std::isfinite(score) || score <= 0.0) continue; + + if (obs.tag_id == active_tag_id_) { + active_obs_index = static_cast(i); + active_score = score; + } + + if (score > best_candidate_score) { + best_candidate_score = score; + best_candidate_tag = obs.tag_id; + best_candidate_index = static_cast(i); + } + } + + bool switched = false; + bool should_switch = false; + + if (active_obs_index < 0) { + ++active_tag_missing_count_; + if ((active_tag_id_ < 0 && best_candidate_index >= 0) || + (active_tag_missing_count_ >= missing_before_switch_ && best_candidate_index >= 0)) { + should_switch = true; + } + } else { + active_tag_missing_count_ = 0; + if (best_candidate_tag >= 0 && + best_candidate_tag != active_tag_id_ && + best_candidate_score > active_score * switch_hysteresis_) { + should_switch = true; + } + } + + if (should_switch && best_candidate_index >= 0) { + switched = (best_candidate_tag != active_tag_id_); + active_tag_id_ = best_candidate_tag; + active_tag_missing_count_ = 0; + active_obs_index = best_candidate_index; + } + + if (active_obs_index < 0) { + last_status_ = Status::NO_MATCHING_TAG; + return false; + } + + const auto& obs = observations_[active_obs_index]; + const auto it = anchors_in_tag_.find(obs.tag_id); + if (it == anchors_in_tag_.end()) { + last_status_ = Status::NO_MATCHING_TAG; + return false; + } + + const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second); + if (!isFiniteVector(p_c)) { + last_status_ = Status::INVALID_INPUT; + return false; + } + + last_p_c_target_ = p_c; + last_used_tag_id_ = obs.tag_id; + last_spread_m_ = 0.0; // 单 tag + 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; - } + if (it == anchors_in_tag_.end()) return false; p_t_target_out = it->second; return true; } +// ========================= +// math helpers +// ========================= +double TagRelativeTarget3D::sanitizeObsWeight(double w) { + return sanitizeWeight(w); +} + +double TagRelativeTarget3D::trackingCandidateScore(const TagPoseObservation& obs) const { + if (obs.tag_id < 0) return -std::numeric_limits::infinity(); + + const auto it = anchors_in_tag_.find(obs.tag_id); + if (it == anchors_in_tag_.end()) return -std::numeric_limits::infinity(); + + const double w = std::max(1e-12, sanitizeObsWeight(obs.weight)); + const 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_); + + const double w_term = std::pow(w, candidate_weight_power_); + const double p_term = std::pow(std::max(1e-12, proximity_gain), candidate_proximity_power_); + + const double score = w_term * p_term; + if (!std::isfinite(score)) return -std::numeric_limits::infinity(); + return score; +} + bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) { - for (int r = 0; r < 4; ++r) { - for (int c = 0; c < 4; ++c) { - if (!std::isfinite(T(r, c))) { - return false; - } - } - } + for (int r = 0; r < 4; ++r) + for (int c = 0; c < 4; ++c) + if (!std::isfinite(T(r, c))) return false; return true; } @@ -245,100 +707,6 @@ bool TagRelativeTarget3D::isFiniteVector(const Eigen::Vector3d& p) { return std::isfinite(p.x()) && std::isfinite(p.y()) && std::isfinite(p.z()); } -bool TagRelativeTarget3D::detectObservationsFromImage(const cv::Mat& color, - const device::Rs2Intrinsics& intrinsics, - std::vector& observations) { - observations.clear(); - - 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(intrinsics.fx); - const double fy = static_cast(intrinsics.fy); - const double cx = static_cast(intrinsics.cx); - const double cy = static_cast(intrinsics.cy); - if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) { - last_status_ = Status::BAD_IMAGE; - return false; - } - - cv::Mat gray; - if (color.channels() == 1) { - gray = color; - } else if (color.channels() == 3) { - cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY); - } else { - cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY); - } - if (gray.empty() || gray.type() != CV_8UC1) { - last_status_ = Status::BAD_IMAGE; - return false; - } - if (!gray.isContinuous()) { - gray = gray.clone(); - } - - const int width = gray.cols; - const int height = gray.rows; - vpCameraParameters cam; - cam.initPersProjWithoutDistortion(fx, fy, cx, cy); - - vpImage I(height, width); - for (int y = 0; y < height; ++y) { - std::memcpy(I[y], gray.ptr(y), static_cast(width)); - } - - std::vector cMo_vec; - const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec); - if (!detected || cMo_vec.empty()) { - last_status_ = Status::NO_TAG; - return false; - } - - const std::vector tag_ids = detector_.getTagsId(); - const std::vector margins = detector_.getTagsDecisionMargin(); - const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size()); - if (pair_size == 0) { - last_status_ = Status::NO_TAG; - return false; - } - - observations.reserve(pair_size); - for (size_t i = 0; i < pair_size; ++i) { - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - const vpHomogeneousMatrix& cMo = cMo_vec[i]; - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - T(r, c) = cMo[r][c]; - } - } - T(0, 3) = cMo[0][3]; - T(1, 3) = cMo[1][3]; - T(2, 3) = cMo[2][3]; - - double w = 1.0; - if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) { - w = static_cast(margins[i]); - } - - observations.push_back(TagPoseObservation{tag_ids[i], T, w}); - } - - if (observations.empty()) { - last_status_ = Status::NO_TAG; - return false; - } - - last_status_ = Status::OK; - return true; -} - Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t) { const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0); @@ -353,4 +721,35 @@ Eigen::Vector3d TagRelativeTarget3D::pointCameraToTag(const Eigen::Matrix4d& T_c 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) { + 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 轴 + const double n_norm = n.norm(); + if (!std::isfinite(n_norm) || n_norm <= 1e-12) return false; + n /= n_norm; + + const double ray_norm = ray_c.norm(); + if (!std::isfinite(ray_norm) || ray_norm <= 1e-12) return false; + 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; + + const double lambda = n.dot(t) / denom; + if (!std::isfinite(lambda) || lambda <= 1e-8) return false; + + p_c_intersection = lambda * d; + if (!isFiniteVector(p_c_intersection)) return false; + + if (view_cos_out) *view_cos_out = std::abs(denom); // [0,1] + return true; +} + } // namespace cmvr::perception diff --git a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp b/cmvr-es/perception/src/tag_relative_target_3d_test.cpp index 5672ff23..ff747f1a 100644 --- a/cmvr-es/perception/src/tag_relative_target_3d_test.cpp +++ b/cmvr-es/perception/src/tag_relative_target_3d_test.cpp @@ -10,19 +10,28 @@ #include #include #include +#include +#include +#include #include +#include +#include #include #include #include +#include +#include namespace { // 直接在这里改测试参数。 constexpr const char* kRsSerial = "243122074587"; -// Target pixel used for depth sampling. Negative means image center. -constexpr int kTargetU = 350; -constexpr int kTargetV = 250; -constexpr double kTagSize = 0.02; +// constexpr const char* kRsSerial = "243122075389"; +// constexpr const char* kRsSerial = "f1421544"; +// Target pixel. Negative means image center. +constexpr int kTargetU = -1; +constexpr int kTargetV = -1; +constexpr double kTagSize = 0.01975 ; // <= 0 means loop until user abort. constexpr int kTries = -1; constexpr int kWidth = 1280; @@ -33,11 +42,7 @@ constexpr int kWarmupMaxTries = 60; constexpr int kWarmupSleepMs = 50; constexpr int kFrameRetryMax = 3; constexpr int kFrameRetrySleepMs = 25; -constexpr int kLogEveryNFrames = 20; -// Depth sample window radius (pixels): -// 1 -> 3x3, 2 -> 5x5, 3 -> 7x7. -constexpr int kDepthSampleRadiusTarget = 3; -constexpr int kDepthSampleRadiusTag = 3; +constexpr int kLogEveryNFrames = 1; struct FramePack { cv::Mat color; @@ -45,37 +50,25 @@ struct FramePack { cmvr::device::Rs2Intrinsics intrinsics{}; }; -bool sampleDepthMeters(const cv::Mat& depth, int u, int v, double& z_m, int radius_px) { - if (depth.empty() || depth.channels() != 1) return false; - if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false; - if (radius_px < 0) return false; - - std::vector valid; - const int side = radius_px * 2 + 1; - valid.reserve(static_cast(side * side)); - for (int dv = -radius_px; dv <= radius_px; ++dv) { - for (int du = -radius_px; du <= radius_px; ++du) { - const int uu = u + du; - const int vv = v + dv; - if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue; - - double z = 0.0; - if (depth.type() == CV_16UC1) { - z = static_cast(depth.at(vv, uu)) * 1e-3; - } else if (depth.type() == CV_32FC1) { - z = static_cast(depth.at(vv, uu)); - } else { - return false; - } - if (std::isfinite(z) && z > 1e-4) { - valid.push_back(z); +bool fetchRGBFrame(const std::shared_ptr& camera, + FramePack& frame, + std::string& error_out) { + error_out.clear(); + for (int retry = 0; retry < kFrameRetryMax; ++retry) { + try { + camera->getRGBImage(frame.color, frame.intrinsics); + if (!frame.color.empty()) { + return true; } + error_out = "empty_frame"; + } catch (const std::exception& e) { + error_out = e.what(); + } + if (retry + 1 < kFrameRetryMax) { + std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs)); } } - if (valid.empty()) return false; - std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end()); - z_m = valid[valid.size() / 2]; - return true; + return false; } bool fetchRGBDFrame(const std::shared_ptr& camera, @@ -88,7 +81,7 @@ bool fetchRGBDFrame(const std::shared_ptr& camera if (!frame.color.empty() && !frame.depth.empty()) { return true; } - error_out = "empty_frame"; + error_out = "empty_rgbd_frame"; } catch (const std::exception& e) { error_out = e.what(); } @@ -120,21 +113,38 @@ bool projectPoint(const Eigen::Vector3d& p_c, return true; } -bool backProjectPixelToCamera(const cmvr::device::Rs2Intrinsics& K, - int u, - int v, - double z_m, - Eigen::Vector3d& p_c) { - if (!std::isfinite(z_m) || z_m <= 1e-6) return false; - if (!std::isfinite(K.fx) || !std::isfinite(K.fy) || - !std::isfinite(K.cx) || !std::isfinite(K.cy) || - K.fx <= 0.0f || K.fy <= 0.0f) { +bool sampleDepthMetersAtPixel(const cv::Mat& depth, int u, int v, double& z_m) { + if (depth.empty() || depth.type() != CV_16UC1) { return false; } - p_c.z() = z_m; - p_c.x() = (static_cast(u) - static_cast(K.cx)) / static_cast(K.fx) * z_m; - p_c.y() = (static_cast(v) - static_cast(K.cy)) / static_cast(K.fy) * z_m; - return true; + if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) { + return false; + } + + std::vector vals; + vals.reserve(9); + constexpr int kRadius = 1; // 3x3 + for (int yy = std::max(0, v - kRadius); yy <= std::min(depth.rows - 1, v + kRadius); ++yy) { + for (int xx = std::max(0, u - kRadius); xx <= std::min(depth.cols - 1, u + kRadius); ++xx) { + const uint16_t raw = depth.at(yy, xx); + if (raw == 0) { + continue; + } + const double z = static_cast(raw) * 1e-3; // mm -> m + if (!std::isfinite(z) || z <= 1e-4 || z >= 50.0) { + continue; + } + vals.push_back(z); + } + } + if (vals.empty()) { + return false; + } + + const size_t mid = vals.size() / 2; + std::nth_element(vals.begin(), vals.begin() + mid, vals.end()); + z_m = vals[mid]; + return std::isfinite(z_m) && z_m > 0.0; } void drawTagAxes(cv::Mat& img, @@ -170,8 +180,97 @@ void drawTagAxes(cv::Mat& img, 1); } +struct DisplayTagPose { + int tag_id{-1}; + Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; +}; + +bool detectTagPosesForDisplay(const cv::Mat& color, + const cmvr::device::Rs2Intrinsics& intrinsics, + double tag_size_m, + std::vector& out_tags) { + out_tags.clear(); + if (color.empty()) { + return false; + } + if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) { + return false; + } + if (!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) || + !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) || + intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) { + 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) { + return false; + } + if (!gray.isContinuous()) { + gray = gray.clone(); + } + + vpCameraParameters cam; + cam.initPersProjWithoutDistortion( + static_cast(intrinsics.fx), + static_cast(intrinsics.fy), + static_cast(intrinsics.cx), + static_cast(intrinsics.cy)); + + vpImage I(gray.rows, gray.cols); + for (int y = 0; y < gray.rows; ++y) { + std::memcpy(I[y], gray.ptr(y), static_cast(gray.cols)); + } + + static vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11); + static bool detector_init = false; + if (!detector_init) { + detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); + detector_init = true; + } + + std::vector cMo_vec; + const bool detected = detector.detect(I, tag_size_m, cam, cMo_vec); + if (!detected || cMo_vec.empty()) { + return false; + } + + const std::vector ids = detector.getTagsId(); + const size_t n = std::min(ids.size(), cMo_vec.size()); + out_tags.reserve(n); + for (size_t i = 0; i < n; ++i) { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + const auto& 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]; + out_tags.push_back(DisplayTagPose{ids[i], T}); + } + return !out_tags.empty(); +} + +std::string formatVec3(const Eigen::Vector3d& p, int precision = 6) { + std::ostringstream oss; + oss << std::fixed << std::setprecision(precision) + << "[" << p.x() << ", " << p.y() << ", " << p.z() << "]"; + return oss.str(); +} + } // namespace + TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { const std::string serial = kRsSerial; if (serial.empty()) { @@ -198,8 +297,8 @@ 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_RGBD); - cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); + cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGB); + // 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); @@ -207,6 +306,9 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { auto camera = std::make_shared(cam_cfg); cmvr::perception::TagRelativeTarget3D tracker(camera); tracker.setTagSize(tag_size); + tracker.setActiveTagSwitchPolicy(4, 1.2); + tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08); + tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); ASSERT_NO_THROW(camera->init()); ASSERT_NO_THROW(camera->start()); @@ -238,22 +340,28 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { bool ok = false; bool user_abort = false; int frame_fail_count = 0; - int detect_success_count = 0; - int target_valid_count = 0; + int plane_valid_count = 0; int lock_success_count = 0; + int track_success_count = 0; + int track_fail_count = 0; + int tag_switch_count = 0; int printed_count = 0; + bool target_uv_initialized = false; + int target_u = -1; + int target_v = -1; + bool target_locked = false; FramePack last_good_frame; last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); bool has_last_good_frame = false; const auto t_loop_start = std::chrono::steady_clock::now(); - // Warmup: try to get RGBD frames after pipeline start. + // Warmup: try to get RGB frames after pipeline start. bool warmup_ok = false; bool warmup_abort = false; for (int i = 0; i < kWarmupMaxTries; ++i) { FramePack warm_frame; std::string warm_error; - warmup_ok = fetchRGBDFrame(camera, warm_frame, warm_error); + warmup_ok = fetchRGBFrame(camera, warm_frame, warm_error); if (warmup_ok) { break; } @@ -275,54 +383,93 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { FramePack frame; frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); std::string frame_error; - const bool frame_ok = fetchRGBDFrame(camera, frame, frame_error); + const bool frame_ok = fetchRGBFrame(camera, frame, frame_error); if (!frame_ok) { ++frame_fail_count; if (has_last_good_frame) { frame.color = last_good_frame.color.clone(); - frame.depth = last_good_frame.depth.clone(); frame.intrinsics = last_good_frame.intrinsics; } else { frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); } } else { last_good_frame.color = frame.color.clone(); - last_good_frame.depth = frame.depth.clone(); last_good_frame.intrinsics = frame.intrinsics; has_last_good_frame = true; } + std::vector vis_tags; + const bool vis_tag_detected = detectTagPosesForDisplay(frame.color, frame.intrinsics, tag_size, vis_tags); - std::vector observations; - const bool detected = tracker.detectObservationsFromImage(frame.color, frame.intrinsics, observations); - const auto status = tracker.lastStatus(); - const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status); - if (detected) { - ++detect_success_count; + if (!target_uv_initialized) { + target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2); + target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2); + target_uv_initialized = true; } Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero(); - const int target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2); - const int target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2); - double z_depth_m = 0.0; - bool target_pc_valid = false; - if (!frame.color.empty()) { - const bool depth_ok = sampleDepthMeters(frame.depth, - target_u, - target_v, - z_depth_m, - kDepthSampleRadiusTarget); - if (depth_ok && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) && - std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) && - frame.intrinsics.fx > 1e-6f && frame.intrinsics.fy > 1e-6f) { - p_c_target.z() = z_depth_m; - p_c_target.x() = (static_cast(target_u) - static_cast(frame.intrinsics.cx)) - / static_cast(frame.intrinsics.fx) * z_depth_m; - p_c_target.y() = (static_cast(target_v) - static_cast(frame.intrinsics.cy)) - / static_cast(frame.intrinsics.fy) * z_depth_m; - target_pc_valid = true; - ++target_valid_count; + int used_tag_id = -1; + double spread_m = 0.0; + bool target_valid = false; + std::string phase = target_locked ? "track" : "lock"; + + if (!target_locked) { + if (tracker.startTrackingFromPixel(target_u, target_v)) { + p_c_target = tracker.lastTargetInCamera(); + used_tag_id = tracker.lastUsedTagId(); + spread_m = tracker.lastSpread(); + target_locked = true; + phase = "track"; + target_valid = true; + ok = true; + ++plane_valid_count; + ++lock_success_count; + ++printed_count; + + std::cout << "[TagRelativeTarget3DTest][LOCK] target_p_c=" + << formatVec3(p_c_target) + << " used_tag=" << used_tag_id + << " active_tag=" << tracker.activeTagId() + << " spread=" << spread_m + << "\n"; + std::cout << "[TagRelativeTarget3DTest] anchors=" << tracker.anchorCount() << "\n"; + } else if (((i + 1) % kLogEveryNFrames) == 0) { + std::cout << "[TagRelativeTarget3DTest] startTracking failed: " + << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()) + << "\n"; + } + } else { + const int old_active_tag = tracker.activeTagId(); + 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; + ++printed_count; + if (switched) { + ++tag_switch_count; + std::cout << "[TagRelativeTarget3DTest][SWITCH] active_tag " + << old_active_tag << " -> " << tracker.activeTagId() << "\n"; + } + std::cout << "[TagRelativeTarget3DTest][TRACK] target_p_c=" + << formatVec3(p_c_target) + << " used_tag=" << used_tag_id + << " active_tag=" << tracker.activeTagId() + << " spread=" << spread_m + << "\n"; + } else { + ++track_fail_count; + if (((i + 1) % kLogEveryNFrames) == 0) { + std::cout << "[TagRelativeTarget3DTest] track failed: " + << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()) + << "\n"; + } } } + const auto status = tracker.lastStatus(); + const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status); cv::Mat vis = frame.color.clone(); cv::drawMarker(vis, @@ -346,7 +493,11 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { cv::Scalar(0, 255, 255), 2); cv::putText(vis, - "tags=" + std::to_string(observations.size()) + + "phase=" + phase + " locked=" + std::to_string(target_locked ? 1 : 0) + + " active_tag=" + std::to_string(tracker.activeTagId()) + + " miss=" + std::to_string(tracker.activeTagMissingCount()) + + " anchors=" + std::to_string(tracker.anchorCount()) + + " vis_tags=" + std::to_string(vis_tag_detected ? vis_tags.size() : 0) + " frame_fail=" + std::to_string(frame_fail_count), cv::Point(12, 84), cv::FONT_HERSHEY_SIMPLEX, @@ -363,23 +514,30 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { 1); } cv::putText(vis, - std::string("depth@target: ") + (target_pc_valid ? std::to_string(z_depth_m) + "m" : "invalid"), + std::string("target3d: ") + (target_valid ? "valid" : "invalid") + + " used_tag=" + std::to_string(used_tag_id) + + " spread=" + std::to_string(spread_m), cv::Point(12, 128), cv::FONT_HERSHEY_SIMPLEX, - 0.5, - target_pc_valid ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255), + 0.45, + target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255), + 1); + cv::putText(vis, + "target_p_c=" + (target_valid ? formatVec3(p_c_target) : "invalid"), + cv::Point(12, 150), + cv::FONT_HERSHEY_SIMPLEX, + 0.45, + target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255), 1); - // Draw detected tag axes (camera frame). - if (detected && !observations.empty()) { + if (vis_tag_detected) { const double axis_len = tag_size * 0.5; - for (const auto& obs : observations) { - drawTagAxes(vis, obs.T_c_t, frame.intrinsics, axis_len, obs.tag_id); + for (const auto& tag : vis_tags) { + drawTagAxes(vis, tag.T_c_t, frame.intrinsics, axis_len, tag.tag_id); } } - // Draw target point (projected from camera coordinates). - if (std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) && + if (target_valid && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) && std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) && p_c_target.z() > 1e-6) { const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx; @@ -387,16 +545,17 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { const int ui = static_cast(std::lround(u)); const int vi = static_cast(std::lround(v)); if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) { - cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 0, 255), 2); + cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 255, 255), 2); cv::putText(vis, "target", - cv::Point(ui + 8, vi - 8), + cv::Point(ui + 8, vi + 16), cv::FONT_HERSHEY_SIMPLEX, 0.5, - cv::Scalar(0, 0, 255), + cv::Scalar(0, 255, 255), 1); } } + try { cv::imshow(kWindowName, vis); const int key = cv::waitKey(1); @@ -410,96 +569,7 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { break; } - if (!detected) { - if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] detect failed: " - << status_str - << " (try=" << (i + 1) << "/" << tries << ")\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - continue; - } - - if (!observations.empty()) { - const double tag_tz = observations.front().T_c_t(2, 3); - if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] t.z=" << tag_tz - << " depth@target=" << (target_pc_valid ? z_depth_m : -1.0) - << " tag_id=" << observations.front().tag_id << "\n"; - } - } - - for (const auto& obs : observations) { - const Eigen::Vector3d p_c_tag_pose = obs.T_c_t.block<3, 1>(0, 3); - cv::Point uv_tag; - double z_tag_depth_m = 0.0; - Eigen::Vector3d p_c_tag_depth = Eigen::Vector3d::Zero(); - const bool uv_ok = projectPoint(p_c_tag_pose, frame.intrinsics, uv_tag); - bool tag_depth_ok = false; - if (uv_ok) { - tag_depth_ok = sampleDepthMeters(frame.depth, - uv_tag.x, - uv_tag.y, - z_tag_depth_m, - kDepthSampleRadiusTag) && - backProjectPixelToCamera(frame.intrinsics, - uv_tag.x, - uv_tag.y, - z_tag_depth_m, - p_c_tag_depth); - } - - std::cout << "[TagRelativeTarget3DTest] tag_id=" << obs.tag_id - << " pose_p_c=[" << p_c_tag_pose.x() << ", " << p_c_tag_pose.y() << ", " << p_c_tag_pose.z() << "]" - << " depth_p_c=" - << (tag_depth_ok - ? "[" + std::to_string(p_c_tag_depth.x()) + ", " + - std::to_string(p_c_tag_depth.y()) + ", " + - std::to_string(p_c_tag_depth.z()) + "]" - : "[invalid]") - << " uv=[" << (uv_ok ? std::to_string(uv_tag.x) : "invalid") - << ", " << (uv_ok ? std::to_string(uv_tag.y) : "invalid") << "]\n"; - } - - std::cout << "[TagRelativeTarget3DTest] target_depth_p_c=" - << (target_pc_valid - ? "[" + std::to_string(p_c_target.x()) + ", " + - std::to_string(p_c_target.y()) + ", " + - std::to_string(p_c_target.z()) + "]" - : "[invalid]") - << " target_uv=[" << target_u << ", " << target_v << "]\n"; - - if (!target_pc_valid) { - if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] skip lock: invalid depth at target pixel\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - continue; - } - - if (!tracker.lockTargetInCamera(p_c_target, observations, true)) { - if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] lock failed: " - << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()) - << "\n"; - } - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - continue; - } - ++lock_success_count; - - std::cout << "[TagRelativeTarget3DTest] detected_tags=" << observations.size() << "\n"; - for (const auto& obs : observations) { - Eigen::Vector3d p_t = Eigen::Vector3d::Zero(); - if (!tracker.getAnchorInTag(obs.tag_id, p_t)) { - continue; - } - std::cout << " tag_id=" << obs.tag_id - << " p_t_target=[" << p_t.x() << ", " << p_t.y() << ", " << p_t.z() << "]\n"; - } - ++printed_count; - - ok = true; + std::this_thread::sleep_for(std::chrono::milliseconds(30)); } try { @@ -514,11 +584,120 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { const auto t_loop_end = std::chrono::steady_clock::now(); const double total_s = std::chrono::duration_cast>(t_loop_end - t_loop_start).count(); std::cout << "[TagRelativeTarget3DTest] summary " - << "detect_ok=" << detect_success_count - << " target_valid=" << target_valid_count + << " plane_valid=" << plane_valid_count << " lock_ok=" << lock_success_count + << " track_ok=" << track_success_count + << " track_fail=" << track_fail_count + << " switch=" << tag_switch_count << " printed=" << printed_count << " frame_fail=" << frame_fail_count << " duration_s=" << total_s << "\n"; } + +TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) { + const std::string serial = kRsSerial; + if (serial.empty()) { + GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp"; + } + + cmvr::config::RealSenseCameraConfig cam_cfg; + cam_cfg.set_id("tag_relative_target_3d_test_no_display"); + cam_cfg.set_serialnumber(serial); + cam_cfg.set_width(kWidth); + cam_cfg.set_height(kHeight); + cam_cfg.set_fps(kFps); + cam_cfg.set_codec("H265"); + cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO); + cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD); + cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); + cam_cfg.set_buffer_size(30); + cam_cfg.set_sync(true); + cam_cfg.set_enable(true); + + auto camera = std::make_shared(cam_cfg); + cmvr::perception::TagRelativeTarget3D tracker(camera); + ASSERT_NO_THROW(camera->init()); + ASSERT_NO_THROW(camera->start()); + struct CameraStopGuard { + std::shared_ptr cam; + ~CameraStopGuard() { + if (!cam) return; + try { + cam->stop(); + } catch (...) { + } + } + } stop_guard{camera}; + tracker.setTagSize(kTagSize); + tracker.setActiveTagSwitchPolicy(4, 1.2); + tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); + // 深度法采样：5x5 中值 + MAD 离群剔除。 + tracker.setDepthSamplingConfig(5, true, 2.5, 0.003); + const int tries = kTries; + const bool infinite = (tries <= 0); + int plane_fail_count = 0; + int depth_fail_count = 0; + int plane_ok_count = 0; + int depth_ok_count = 0; + int both_ok_count = 0; + int printed_count = 0; + bool got_target = false; + const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2); + const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2); + + for (int i = 0; infinite || i < tries; ++i) { + // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。 + tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); + const bool plane_ok = tracker.solveFromPixel(target_u, target_v); + const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); + const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1; + const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0; + if (plane_ok) { + ++plane_ok_count; + } else { + ++plane_fail_count; + } + + tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE); + const bool depth_ok = tracker.solveFromPixel(target_u, target_v); + const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); + if (depth_ok) { + ++depth_ok_count; + } else { + ++depth_fail_count; + } + + if (plane_ok && depth_ok) { + ++both_ok_count; + } + if (plane_ok || depth_ok) { + got_target = true; + } + + std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] " + << "uv=[" << target_u << "," << target_v << "] " + << "plane_ok=" << plane_ok + << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]") + << " used_tag=" << plane_used_tag + << " spread=" << plane_spread + << " | depth_ok=" << depth_ok + << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]"); + if (plane_ok && depth_ok) { + std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm(); + } + std::cout << std::endl; + + ++printed_count; + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + } + + EXPECT_TRUE(got_target) + << "Failed to solve target point in camera frame. " + << " plane_fail=" << plane_fail_count + << " depth_fail=" << depth_fail_count + << " plane_ok=" << plane_ok_count + << " depth_ok=" << depth_ok_count + << " both_ok=" << both_ok_count + << " printed=" << printed_count; +} diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/Bundle.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/Bundle.h new file mode 100644 index 00000000..8cf7873c --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/Bundle.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_BUNDLE_H_ +#define _MANIF_BUNDLE_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/bundle/Bundle_properties.h" +#include "manif/impl/bundle/Bundle_base.h" +#include "manif/impl/bundle/Bundle_map.h" +#include "manif/impl/bundle/Bundle.h" +#include "manif/impl/bundle/BundleTangent_base.h" +#include "manif/impl/bundle/BundleTangent.h" +#include "manif/impl/bundle/BundleTangent_map.h" + +#endif // _MANIF_BUNDLE_H_ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/Rn.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/Rn.h new file mode 100644 index 00000000..1affac10 --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/Rn.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_RN_H_ +#define _MANIF_RN_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/rn/Rn_properties.h" +#include "manif/impl/rn/Rn_base.h" +#include "manif/impl/rn/RnTangent_base.h" +#include "manif/impl/rn/Rn.h" +#include "manif/impl/rn/RnTangent.h" +#include "manif/impl/rn/Rn_map.h" +#include "manif/impl/rn/RnTangent_map.h" + +#endif // _MANIF_RN_H_ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SE2.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE2.h new file mode 100644 index 00000000..c0195140 --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE2.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SE2_H_ +#define _MANIF_SE2_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/se2/SE2_properties.h" +#include "manif/impl/se2/SE2_base.h" +#include "manif/impl/se2/SE2Tangent_base.h" +#include "manif/impl/se2/SE2.h" +#include "manif/impl/se2/SE2Tangent.h" +#include "manif/impl/se2/SE2_map.h" +#include "manif/impl/se2/SE2Tangent_map.h" + +#endif /* _MANIF_SE2_H_ */ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SE3.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE3.h new file mode 100644 index 00000000..0910462c --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SE3_H_ +#define _MANIF_SE3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/se3/SE3_properties.h" +#include "manif/impl/se3/SE3_base.h" +#include "manif/impl/se3/SE3Tangent_base.h" +#include "manif/impl/se3/SE3.h" +#include "manif/impl/se3/SE3Tangent.h" +#include "manif/impl/se3/SE3_map.h" +#include "manif/impl/se3/SE3Tangent_map.h" + +#endif /* _MANIF_SE3_H_ */ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SE_2_3.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE_2_3.h new file mode 100644 index 00000000..8cdf7d35 --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SE_2_3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SE_2_3_H_ +#define _MANIF_SE_2_3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/se_2_3/SE_2_3_properties.h" +#include "manif/impl/se_2_3/SE_2_3_base.h" +#include "manif/impl/se_2_3/SE_2_3Tangent_base.h" +#include "manif/impl/se_2_3/SE_2_3.h" +#include "manif/impl/se_2_3/SE_2_3Tangent.h" +#include "manif/impl/se_2_3/SE_2_3_map.h" +#include "manif/impl/se_2_3/SE_2_3Tangent_map.h" + +#endif /* _MANIF_SE_2_3_H_ */ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SGal3.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SGal3.h new file mode 100644 index 00000000..895e29ea --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SGal3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SGAL3_H_ +#define _MANIF_SGAL3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/sgal3/SGal3_properties.h" +#include "manif/impl/sgal3/SGal3_base.h" +#include "manif/impl/sgal3/SGal3Tangent_base.h" +#include "manif/impl/sgal3/SGal3.h" +#include "manif/impl/sgal3/SGal3Tangent.h" +#include "manif/impl/sgal3/SGal3_map.h" +#include "manif/impl/sgal3/SGal3Tangent_map.h" + +#endif // _MANIF_SGAL3_H_ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SO2.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SO2.h new file mode 100644 index 00000000..0091a0a1 --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SO2.h @@ -0,0 +1,17 @@ +#ifndef _MANIF_SO2_H_ +#define _MANIF_SO2_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/utils.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/so2/SO2_properties.h" +#include "manif/impl/so2/SO2_base.h" +#include "manif/impl/so2/SO2Tangent_base.h" +#include "manif/impl/so2/SO2.h" +#include "manif/impl/so2/SO2Tangent.h" +#include "manif/impl/so2/SO2_map.h" +#include "manif/impl/so2/SO2Tangent_map.h" + +#endif /* _MANIF_SO2_H_ */ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/SO3.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/SO3.h new file mode 100644 index 00000000..6e2100fb --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/SO3.h @@ -0,0 +1,16 @@ +#ifndef _MANIF_SO3_H_ +#define _MANIF_SO3_H_ + +#include "manif/impl/macro.h" +#include "manif/impl/lie_group_base.h" +#include "manif/impl/tangent_base.h" + +#include "manif/impl/so3/SO3_properties.h" +#include "manif/impl/so3/SO3_base.h" +#include "manif/impl/so3/SO3Tangent_base.h" +#include "manif/impl/so3/SO3.h" +#include "manif/impl/so3/SO3Tangent.h" +#include "manif/impl/so3/SO3_map.h" +#include "manif/impl/so3/SO3Tangent_map.h" + +#endif /* _MANIF_SO3_H_ */ diff --git a/dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/average.h b/dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/average.h new file mode 100644 index 00000000..acb40d05 --- /dev/null +++ b/dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/average.h @@ -0,0 +1,274 @@ +#ifndef _MANIF_MANIF_AVERAGE_H_ +#define _MANIF_MANIF_AVERAGE_H_ + +#include "manif/impl/lie_group_base.h" +//#include "manif/interpolation.h" +#include +namespace manif { + +//template