SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add ibvs real robot test

Browse Source
This commit is contained in:
lgv 2026-02-27 14:49:54 +08:00
parent b218fdccf8
commit 021b14b387
42 changed files with 1748 additions and 660 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmvr-es/CMakeLists.txt
View File

@ -7,6 +7,7 @@ add_subdirectory(monitor_manager)
add_subdirectory(service)
add_subdirectory(controller)
add_subdirectory(planner)
add_subdirectory(perception)
add_subdirectory(ik_solver)
add_subdirectory(data_center)

2
cmvr-es/controller/CMakeLists.txt
View File

@ -39,7 +39,7 @@ target_link_libraries(controller PUBLIC
add_library(cmvr_es::controller ALIAS controller)
install(TARGETS controller LIBRARY DESTINATION lib)
# --------------------------------------------------------

24
cmvr-es/controller/include/ibvs_controller.h
View File

@ -47,6 +47,7 @@ public:
INVALID_INPUT, /**< 输入参数异常。 */
NO_DEPTH, /**< 需要深度但深度不可用。 */
NO_TAG, /**< 未检测到 AprilTag。 */
TAG_MISMATCH, /**< 检测到 AprilTag，但与指定 id 不匹配。 */
IK_FAILED /**< 速度 IK 求解失败。 */
};
@ -116,7 +117,12 @@ public:
*/
void setTagSize(double tag_size_m);
/**
* @brief 设置期望目标位姿（`cMo_des`）。
* @brief 设置要跟踪的 tag id（必填）。
* @param tag_id 指定 tag id，必须 >= 0。
*/
void setTrackedTagId(int tag_id);
/**
* @brief 设置期望目标位姿（`cMo_des tag 坐标系相对于VISP相机的期望位姿`）。
* @param x 期望平移 x（米）。
* @param y 期望平移 y（米）。
* @param z 期望平移 z（米）。
@ -167,7 +173,7 @@ public:
void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
/**
* @brief 最近一帧是否检测到 tag。
* @brief 最近一帧是否检测到“指定 id 的 tag”。
* @return 检测到返回 `true`。
*/
bool isTagDetected() const { return last_tag_detected_; }
@ -198,6 +204,16 @@ public:
* @return tag 平移向量。
*/
const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
/**
* @brief 获取当前指定的 tag id。
* @return 指定的 tag id；若未设置返回 `-1`。
*/
int trackedTagId() const { return tracked_tag_id_; }
/**
* @brief 获取最近一次用于控制的 tag id。
* @return 最近使用的 tag id；若本帧未成功使用返回 `-1`。
*/
int lastUsedTagId() const { return last_used_tag_id_; }
/**
* @brief 获取最近输出的相机 twist（ViSP 相机坐标系）。
* @return 六维 twist 向量。
@ -279,9 +295,13 @@ private:
vpFeaturePoint s_star_[4];
// AprilTag 检测器。
vpDetectorAprilTag detector_;
// 指定跟踪的 tag id（必填）；<0 表示未设置。
int tracked_tag_id_{-1};
// 速度 IK 求解器。
std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
// 最近一次用于控制的 tag id。
int last_used_tag_id_{-1};
// 内部积分得到的关节位置命令缓存。
std::vector<double> q_cmd_;

4
cmvr-es/controller/src/controller_test.cpp
View File

@ -539,7 +539,8 @@ protected:
ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR);
ibvs_controller_->setDepthZGain(1.0);
ibvs_controller_->setVelocityLimit6(vmax6_);
ibvs_controller_->setTarget(0.0,0.0,0.15);
ibvs_controller_->setTrackedTagId(tracked_tag_id_);
ibvs_controller_->setTarget(0.0,0.0,0.35);
const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
1, 0, 0,
@ -695,6 +696,7 @@ private:
std::string urdf_path_for_check_{
"/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};
std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};

50
cmvr-es/controller/src/ibvs_controller.cpp
View File

@ -10,6 +10,7 @@
#include <algorithm>
#include <cmath>
#include <cstring>
#include <iostream>
#include <limits>
#include <opencv2/imgproc.hpp>
@ -29,6 +30,7 @@ const char* IbvsController::statusToString(ComputeStatus status) {
case ComputeStatus::INVALID_INPUT: return "invalid_input";
case ComputeStatus::NO_DEPTH: return "no_depth";
case ComputeStatus::NO_TAG: return "no_tag";
case ComputeStatus::TAG_MISMATCH: return "tag_mismatch";
case ComputeStatus::IK_FAILED: return "ik_failed";
default: return "unknown";
}
@ -91,6 +93,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
q_cmd_ = q_init;
}
last_tag_detected_ = false;
last_used_tag_id_ = -1;
last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
last_depth_usage_ = DepthUsage::NONE;
last_tag_pos_visp_.setZero();
@ -161,16 +164,53 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
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);
last_tag_detected_ = (detected && !cMo_vec.empty());
if (!last_tag_detected_) {
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;
}
vpHomogeneousMatrix cMo = cMo_vec[0];
// 只允许使用指定 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）:
@ -309,6 +349,10 @@ void IbvsController::setTagSize(double tag_size_m) {
initTask();
}
void IbvsController::setTrackedTagId(int tag_id) {
tracked_tag_id_ = tag_id;
}
void IbvsController::setTarget(double x,
double y,
double z,

3
cmvr-es/data_center/CMakeLists.txt
View File

@ -14,3 +14,6 @@ target_link_libraries(data_center PRIVATE
protobuf
glog
)
install(TARGETS data_center LIBRARY DESTINATION lib)

1
cmvr-es/device_manager/CMakeLists.txt
View File

@ -20,3 +20,4 @@ target_link_libraries(device_manager PRIVATE
)
add_library(cmvr_es::device_manager ALIAS device_manager)
install(TARGETS device_manager LIBRARY DESTINATION lib)

9
cmvr-es/devices/camera/realsense_camera/CMakeLists.txt
View File

@ -5,7 +5,14 @@ target_include_directories(realsense_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK
# 链接 librealsense2
target_link_libraries(realsense_camera PRIVATE realsense2)
target_link_libraries(realsense_camera PRIVATE
realsense2
avcodec
avformat
avutil
swscale
swresample
)
install(TARGETS realsense_camera LIBRARY DESTINATION lib)
# --------------------------------------------------------

183
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -151,23 +151,41 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
auto stream_mode = camera_.stream_mode();
if (stream_mode == config::STREAM_MODE_RGB)
{
stream_mode_ = RGBD_MODE;
stream_mode_ = COLOR_MODE;
}
else if (stream_mode == config::STREAM_MODE_RGBD)
{
stream_mode_ = COLOR_MODE;
stream_mode_ = RGBD_MODE;
}
else if (stream_mode == config::STREAM_MODE_DEPTH)
{
stream_mode_ = DEPTH_MODE;
}
else {
stream_mode_ = RGBD_MODE;
}
const auto align_mode = camera_.align_mode();
if (align_mode == config::ALIGN_MODE_COLOR) {
align_mode_ = "color";
} else if (align_mode == config::ALIGN_MODE_DEPTH) {
align_mode_ = "depth";
} else {
align_mode_ = "color";
}
codec_ = camera_.codec();
}
RealsenseCamera::~RealsenseCamera() {
// 停止采集线程，释放资源
stop();
// 析构阶段禁止抛异常，避免 terminate。
try {
stop();
} catch (const std::exception& e) {
LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): " << e.what();
} catch (...) {
LOG(WARNING) << "[RealsenseCamera] (~RealsenseCamera): unknown exception";
}
}
void RealsenseCamera::init() {
@ -222,78 +240,110 @@ void RealsenseCamera::start() {
throw runtime_error("Camera not initialized");
}
try{
if (state_.is_opened) {
LOG(WARNING) << "[RealsenseCamera] (start): camera already started";
if (state_.is_opened) {
LOG(WARNING) << "[RealsenseCamera] (start): camera already started";
return;
}
constexpr int kStartMaxRetry = 3;
constexpr int kRetrySleepMs = 300;
constexpr unsigned int kProbeTimeoutMs = 2000;
std::string last_error;
state_.is_opened = false;
for (int attempt = 1; attempt <= kStartMaxRetry; ++attempt) {
try {
// 每次尝试前重建 pipeline，避免复用异常状态。
pipe_ = rs2::pipeline();
// 使用相同的同步策略启动
profile_ = pipe_.start(rs_cfg_);
if (stream_mode_ == RGBD_MODE){
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
Kc_ = sp_color.get_intrinsics();
Ec2d_ = sp_color.get_extrinsics_to(sp_depth);
Ed2c_ = sp_depth.get_extrinsics_to(sp_color);
intrinsics_ = Kc_;
}
else if (stream_mode_ == COLOR_MODE) {
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kc_ = sp_color.get_intrinsics();
intrinsics_ = Kc_;
}
else if (stream_mode_ == DEPTH_MODE) {
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
intrinsics_ = Kd_;
}
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
}
// 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。
rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs);
if (!probe.get_color_frame()) {
throw runtime_error("no color frame after start");
}
if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) {
throw runtime_error("no depth frame after start");
}
state_.is_opened = true;
LOG(INFO) << "realsense start streaming successfully";
return;
}
// 使用相同的同步策略启动
profile_ = pipe_.start(rs_cfg_);
if (stream_mode_ == RGBD_MODE){
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
Kc_ = sp_color.get_intrinsics();
Ec2d_ = sp_color.get_extrinsics_to(sp_depth);
Ed2c_ = sp_depth.get_extrinsics_to(sp_color);
intrinsics_ = Kc_;
catch (const exception &error) {
last_error = error.what();
state_.is_opened = false;
LOG(WARNING) << "[RealsenseCamera] (start): attempt "
<< attempt << "/" << kStartMaxRetry
<< " failed: " << last_error;
try {
pipe_.stop();
} catch (...) {
// ignore stop exceptions during retry cleanup
}
align_.reset();
std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs));
}
else if (stream_mode_ == COLOR_MODE) {
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kc_ = sp_color.get_intrinsics();
intrinsics_ = Kc_;
}
else if (stream_mode_ == DEPTH_MODE) {
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
intrinsics_ = Kd_;
}
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
}
// auto frames = get_frameset(true);
// rs2::frame color_frame = frames.get_color_frame();
// auto profile = color_frame.get_profile();
// auto color_profile = frames.get_color_frame().get_profile();
// intrinsics_ = color_profile.as<rs2::video_stream_profile>().get_intrinsics();
state_.is_opened = true;
LOG(INFO) << "realsense start streaming successfully";
}
catch (const exception &error) {
state_.is_opened = false;
LOG(ERROR) << "realsense start streaming error: " << error.what();
}
LOG(ERROR) << "realsense start streaming error: " << last_error;
throw runtime_error("realsense start streaming error: " + last_error);
}
void RealsenseCamera::stop() {
//先停止录制再关闭摄像头
if (state_.is_recording) {
stopRecording();
try {
stopRecording();
} catch (const std::exception& e) {
LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what();
}
}
std::lock_guard lock(ctrl_mtx_);
clear_error_();
try {
if (!state_.is_opened || !state_.is_initialized) {
throw runtime_error("Camera already closed");
}
pipe_.stop();
if (!state_.is_opened || !state_.is_initialized) {
state_.is_opened = false;
return;
}
catch (exception &e) {
LOG(ERROR) << "[RealsenseCamera] (stop): " << e.what();
throw runtime_error(e.what());
try {
pipe_.stop();
} catch (const std::exception& e) {
LOG(WARNING) << "[RealsenseCamera] (stop): " << e.what();
}
align_.reset();
pipe_ = rs2::pipeline();
state_.is_opened = false;
}
void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
@ -337,7 +387,7 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
latest_depth_ = depth;
}
}
LOG(INFO) << "realsense get rgb frame successfully";
// LOG(INFO) << "realsense get rgb frame successfully";
}
catch (const std::exception& e) {
throw std::runtime_error("[RealsenseCamera] (getRGBImage): Failed to get image: " + std::string(e.what()));
@ -619,7 +669,8 @@ void RealsenseCamera::resumeRecording() {
rs2::frameset RealsenseCamera::get_frameset(bool align) {
std::lock_guard<std::mutex> lock(frame_mtx_);
rs2::frameset frames = pipe_.wait_for_frames();
constexpr unsigned int kWaitTimeoutMs = 300;
rs2::frameset frames = pipe_.wait_for_frames(kWaitTimeoutMs);
// 如果需要对齐，添加对齐逻辑
if (align_ && align && stream_mode_ == RGBD_MODE) {
frames = align_->process(frames);
@ -842,9 +893,8 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
av_opt_set(ctx->priv_data, "annexb", "1", 0);
} else if (codec->id == AV_CODEC_ID_HEVC) {
// 直接使用默认参数，不自定义
// av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
// av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
// 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
}
// 6. 设置像素格式（不变）
@ -1124,4 +1174,3 @@ Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) {
return { point[0], point[1], point[2] };
}

175
cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp
View File

@ -3,8 +3,12 @@
//
#include <gtest/gtest.h>
#include "../../abstract_camera.h"
#include "../include/realsense_camera.h"
#include <opencv2/opencv.hpp>
#include "../../../../device_manager/include/device_manager.h"
#include <chrono>
#include <iostream>
#include <thread>
using namespace cmvr::device;
// 测试: 实时显示 RGB + Depth
TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
@ -35,3 +39,174 @@ TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
LOG(INFO) << "Pose : " << pose ;
cam->stop();
}
namespace {
constexpr const char* kRsSerialForHealthCheck = "243122074587";
constexpr int kRsWidth = 640;
constexpr int kRsHeight = 480;
constexpr int kRsFps = 30;
constexpr int kWarmupMaxTries = 80;
constexpr int kWarmupSleepMs = 30;
cmvr::config::RealSenseCameraConfig makeRsConfig() {
cmvr::config::RealSenseCameraConfig cfg;
cfg.set_id("realsense_health_check");
cfg.set_serialnumber(kRsSerialForHealthCheck);
cfg.set_width(kRsWidth);
cfg.set_height(kRsHeight);
cfg.set_fps(kRsFps);
cfg.set_codec("H265");
cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
cfg.set_buffer_size(30);
cfg.set_sync(true);
cfg.set_enable(true);
return cfg;
}
bool tryGetFirstFrame(const std::shared_ptr<RealsenseCamera>& camera,
cv::Mat& color,
cv::Mat& depth,
Rs2Intrinsics& intrinsics,
int& exception_count,
std::string* last_error = nullptr) {
exception_count = 0;
if (last_error) {
last_error->clear();
}
for (int i = 0; i < kWarmupMaxTries; ++i) {
try {
camera->getRGBDImages(color, depth, intrinsics);
if (!color.empty() && !depth.empty()) {
return true;
}
} catch (const std::exception& e) {
++exception_count;
if (last_error) {
*last_error = e.what();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs));
}
return false;
}
struct RealsenseStopGuard {
std::shared_ptr<RealsenseCamera> camera;
~RealsenseStopGuard() {
if (!camera) return;
try {
camera->stop();
} catch (...) {
}
}
};
} // namespace
TEST(RealsenseCameraHealthCheck, StartAndGetFirstFrame) {
if (std::string(kRsSerialForHealthCheck).empty()) {
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
}
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
ASSERT_NO_THROW(camera->init());
ASSERT_NO_THROW(camera->start());
RealsenseStopGuard stop_guard{camera};
CameraState state{};
camera->getState(state);
EXPECT_TRUE(state.is_initialized);
EXPECT_TRUE(state.is_opened);
cv::Mat color, depth;
Rs2Intrinsics intrinsics{};
int exception_count = 0;
std::string last_error;
const auto t0 = std::chrono::steady_clock::now();
const bool ok = tryGetFirstFrame(camera, color, depth, intrinsics, exception_count, &last_error);
const auto t1 = std::chrono::steady_clock::now();
const auto first_frame_ms = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
std::cout << "[RealsenseCameraHealthCheck] first_frame_ok=" << ok
<< " first_frame_ms=" << first_frame_ms
<< " exceptions=" << exception_count
<< " last_error=" << last_error << "\n";
ASSERT_TRUE(ok) << "Failed to get first RGBD frame after start";
EXPECT_EQ(color.type(), CV_8UC3);
EXPECT_TRUE(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
}
TEST(RealsenseCameraHealthCheck, RGBDReadLoopStability) {
if (std::string(kRsSerialForHealthCheck).empty()) {
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
}
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
ASSERT_NO_THROW(camera->init());
ASSERT_NO_THROW(camera->start());
RealsenseStopGuard stop_guard{camera};
cv::Mat color, depth;
Rs2Intrinsics intrinsics{};
int warmup_exceptions = 0;
ASSERT_TRUE(tryGetFirstFrame(camera, color, depth, intrinsics, warmup_exceptions))
<< "Warmup failed before stability loop";
constexpr int kReadLoopFrames = 150;
int success = 0;
int fail = 0;
int exceptions = 0;
for (int i = 0; i < kReadLoopFrames; ++i) {
try {
camera->getRGBDImages(color, depth, intrinsics);
if (!color.empty() && !depth.empty()) {
++success;
} else {
++fail;
}
} catch (const std::exception&) {
++fail;
++exceptions;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
std::cout << "[RealsenseCameraHealthCheck] read_loop success=" << success
<< " fail=" << fail
<< " exceptions=" << exceptions << "\n";
EXPECT_GT(success, 0);
EXPECT_GE(success, fail);
}
TEST(RealsenseCameraHealthCheck, RestartWithoutReplug) {
if (std::string(kRsSerialForHealthCheck).empty()) {
GTEST_SKIP() << "kRsSerialForHealthCheck is empty";
}
auto camera = std::make_shared<RealsenseCamera>(makeRsConfig());
ASSERT_NO_THROW(camera->init());
RealsenseStopGuard stop_guard{camera};
for (int cycle = 0; cycle < 2; ++cycle) {
ASSERT_NO_THROW(camera->start()) << "start failed at cycle " << cycle;
cv::Mat color, depth;
Rs2Intrinsics intrinsics{};
int exception_count = 0;
std::string last_error;
const bool ok = tryGetFirstFrame(camera, color, depth, intrinsics, exception_count, &last_error);
std::cout << "[RealsenseCameraHealthCheck] cycle=" << cycle
<< " first_frame_ok=" << ok
<< " exceptions=" << exception_count
<< " last_error=" << last_error << "\n";
EXPECT_TRUE(ok) << "Failed to get frame at cycle " << cycle;
EXPECT_NO_THROW(camera->stop()) << "stop failed at cycle " << cycle;
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
}

51
cmvr-es/devices/robot/abstract_robot.h
View File

@ -68,9 +68,32 @@ namespace cmvr::device{
virtual void eStop() { throw std::runtime_error("Not implemented"); }
/**
* @brief 关节空间 point-to-point 运动（内部轨迹规划）。
* @details 控制器会在内部生成满足速度/加速度约束的关节轨迹，并做多关节时间同步，使关节协调到达目标点。
* @param joints 目标关节角，单位 rad。
* @param max_vel 主导轴最大速度，单位 rad/s。
* @param max_acc 主导轴最大加速度，单位 rad/s^2。
*/
virtual void moveJ(std::vector<double> &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); }
/**
* @brief 按关节命令执行关节空间运动（内部轨迹规划）。
* @param cmd 目标关节命令（关节名 + 角度，`vel` 字段可由具体实现决定是否使用）。
* @param vel 主导轴最大速度，单位 rad/s。
* @param acc 主导轴最大加速度，单位 rad/s^2。
*/
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
/**
* @brief 末端位姿目标的关节空间运动。
* @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。
* @param base_link 基坐标系 link 名称。
* @param ee_link 末端 link 名称。
* @param pose 目标末端位姿。
* @param vel 主导轴最大速度，单位 rad/s。
* @param acc 主导轴最大加速度，单位 rad/s^2。
*/
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
@ -97,10 +120,38 @@ namespace cmvr::device{
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
/**
* @brief 关节空间周期伺服（无整段轨迹规划）。
* @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。
* @param joints 当前周期目标关节角，单位 rad。
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
*/
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
/**
* @brief 按关节命令执行周期伺服（无整段轨迹规划）。
* @param joints 当前周期关节目标命令。
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
*/
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
/**
* @brief 带速度参数的关节空间周期伺服。
* @param joints 当前周期关节目标命令。
* @param vel 速度参数（语义由具体驱动实现决定）。
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
*/
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
/**
* @brief 基于末端位姿目标的周期伺服。
* @details 典型实现为每周期执行 IK 并下发关节伺服目标；属于实时闭环接口，不等价于一次性到位的 `moveJ`。
* @param base_link 基坐标系 link 名称。
* @param ee_link 末端 link 名称。
* @param pose 当前周期目标末端位姿。
* @param vel 速度参数（语义由具体驱动实现决定）。
* @param acc 加速度参数（语义由具体驱动实现决定）。
*/
virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }

1
cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt
View File

@ -29,6 +29,7 @@ add_executable(humanoid_robot_test
target_link_libraries(humanoid_robot_test
PRIVATE
cmvr_es::device_manager
cmvr_es::controller
cmvr_es::common
cmvr_es::data_center
osqp

754
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -10,21 +10,57 @@
#include <thread>
#include <chrono>
#include <iostream>
#include <unordered_map>
#include <array>
#include <algorithm>
#include <limits>
#include "gtest/gtest.h"
#include <glog/logging.h>
#include "../../../../device_manager/include/device_manager.h"
#include <libgen.h>
#include "controller/include/ibvs_controller.h"
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "../include/humanoid_robot.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "../../../../utils/base/include/abstract_interpolation.h"
// 定义一个命令行参数 --config_path
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
using namespace cmvr::device;
namespace {
constexpr std::array<const char*, 7> kRightArmJointNames = {
"R_SHOULDER_P",
"R_SHOULDER_R",
"R_SHOULDER_Y",
"R_ELBOW_R",
"R_WRIST_P",
"R_WRIST_Y",
"R_WRIST_R"
};
std::vector<JointPoint> buildRightArmJointCmd(const std::vector<double>& q_cmd,
double vel) {
std::vector<JointPoint> cmd;
cmd.reserve(kRightArmJointNames.size());
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel);
}
return cmd;
}
std::unordered_map<std::string, double> makeRightArmQMap() {
std::unordered_map<std::string, double> q_map;
q_map.reserve(kRightArmJointNames.size());
for (const auto* name : kRightArmJointNames) {
q_map.emplace(name, 0.0);
}
return q_map;
}
} // namespace
TEST(HumanoidRobotTest,GetState) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
@ -71,616 +107,176 @@ TEST(HumanoidRobotTest,MyRobotTest) {
}
TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
// 读取轨迹
std::ifstream file("/home/lgv/cmvr/cmvr-es/src/devices/robot/humanoid_robot/joint_positions_1.csv");
if (!file.is_open()) {
LOG(ERROR) << "无法打开文件" ;
}
std::string line;
// 读取标题行，获取关节名称（除去第一列 idx）
if (!std::getline(file, line)) {
LOG(ERROR) << "文件为空或格式错误" ;
}
std::vector<std::string> joint_names;
{
std::stringstream ss(line);
std::string cell;
// 第一列是 idx，跳过
std::getline(ss, cell, ',');
// 读取关节名称列
while (std::getline(ss, cell, ',')) {
joint_names.push_back(cell);
auto q_map_now = makeRightArmQMap();
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
auto refreshRightArmQ = [&]() -> bool {
robot->getJointQ(q_map_now);
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
const auto it = q_map_now.find(kRightArmJointNames[i]);
if (it == q_map_now.end()) return false;
q_now[i] = it->second;
}
}
return true;
};
std::vector<std::vector<JointPoint>> traj;
// 读取后续每行数据
while (std::getline(file, line)) {
std::stringstream ss(line);
std::string cell;
// 读取第一列 idx，暂时不使用
std::getline(ss, cell, ',');
std::vector<JointPoint> joints;
// 读取每个关节角度
for (size_t i = 0; i < joint_names.size(); ++i) {
if (!std::getline(ss, cell, ',')) {
LOG(ERROR) << "数据列不足，格式错误";
}
double angle = std::stod(cell); // 字符串转 double
JointPoint cmd;
cmd.joint_name = joint_names[i];
cmd.rad = angle;
joints.push_back(cmd);
}
traj.push_back(joints);
}
file.close();
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
// 计算速度
double dt = 0.01; // 采样周期(s)
size_t N = traj.size();
for (size_t i = 0; i < N; ++i) {
if (i == 0) {
// 第一个点没有前一帧，速度设为 0
for (auto &cmd : traj[i]) {
cmd.vel = 0.0;
}
} else {
// 后向差分
for (size_t j = 0; j < traj[i].size(); ++j) {
double pos_prev = traj[i - 1][j].rad;
double pos_curr = traj[i][j].rad;
traj[i][j].vel = (pos_curr - pos_prev) / dt;
}
}
}
// // 测试打印读取结果
// for (size_t i = 0; i < traj.size(); ++i) {
// std::cout << "Index " << i << ":" << std::endl;
// for (const auto& cmd : traj[i]) {
// std::cout << cmd.joint_name
// << " = " << cmd.rad << " rad, "
// << cmd.vel << " rad/s; ";
// }
// std::cout << std::endl;
// }
// 1：
//
// std::vector<JointPoint> cmd{};
// cmd = {
// // {"L_SHOULDER_P", 0.0},
// // {"L_SHOULDER_R", 0.0},
// // {"L_SHOULDER_Y", 0.0},
// // {"L_ELBOW_R", 0.0},
// // {"L_WRIST_P", 0.0},
// // {"L_WRIST_Y", 0.0},
// // {"L_WRIST_R", 0.0},
//
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
// {"R_WRIST_R", 0.0}
// {"R_WRIST_R", 0.0},
// };
// robot->moveJ(cmd,0.8);
// // // 先到达轨迹起点
// LOG(INFO) << "Moving to trajectory start...";
// robot->moveJ(traj[0],0.8);
//
//
//
// // 再移动
// LOG(INFO) << "Reached trajectory start point";
// robot->followJointTrajectory(traj,dt * 1000);
// LOG(INFO) << "Trajectory execution completed";
// std::vector<JointPoint> cmd1{
// {"L_SHOULDER_P", -0.747573},
// {"L_SHOULDER_R", -1.26911},
// {"L_SHOULDER_Y", -1.20811},
// {"L_ELBOW_R", -1.51221},
// {"L_WRIST_P", 2.64099},
// {"L_WRIST_Y", 0.417608},
// {"L_WRIST_R", -0.518287},
//
// {"R_SHOULDER_P", -0.344938},
// {"R_SHOULDER_R", 0.935147},
// {"R_SHOULDER_Y", 2.27031},
// {"R_ELBOW_R", 1.68959},
// {"R_WRIST_P", -2.32841},
// {"R_WRIST_Y", 0.460145},
// {"R_WRIST_R", 0.300996}
// };
//
// std::vector<JointPoint> cmd2{
// {"L_SHOULDER_P", -0.747573},
// {"L_SHOULDER_R", -1.26911},
// {"L_SHOULDER_Y", -1.20811},
// {"L_ELBOW_R", -1.51221},
// {"L_WRIST_P", 2.64099},
// {"L_WRIST_Y", 0.417608},
// {"L_WRIST_R", -0.518287},
//
// {"R_SHOULDER_P", 0.239368},
// {"R_SHOULDER_R", 0.871341},
// {"R_SHOULDER_Y", 1.86052},
// {"R_ELBOW_R", 1.28044},
// {"R_WRIST_P", -2.49436},
// {"R_WRIST_Y", 0.404731},
// {"R_WRIST_R", 0.280016}
// };
//
// std::vector<JointPoint> cmd1{
// {"L_SHOULDER_P", -0.747573},
// {"L_SHOULDER_R", -1.26911},
// {"L_SHOULDER_Y", -1.20811},
// {"L_ELBOW_R", -1.51221},
// {"L_WRIST_P", 2.64099},
// {"L_WRIST_Y", 0.417608},
// {"L_WRIST_R", -0.518287},
//
// {"R_SHOULDER_P", -0.956276},
// {"R_SHOULDER_R", 1.0244},
// {"R_SHOULDER_Y", 2.70621},
// {"R_ELBOW_R", 2.02276},
// {"R_WRIST_P", -1.99653},
// {"R_WRIST_Y", 0.68523},
// {"R_WRIST_R", 0.477066}
// };
//
// std::vector<JointPoint> cmd2{
// {"L_SHOULDER_P", -0.747573},
// {"L_SHOULDER_R", -1.26911},
// {"L_SHOULDER_Y", -1.20811},
// {"L_ELBOW_R", -1.51221},
// {"L_WRIST_P", 2.64099},
// {"L_WRIST_Y", 0.417608},
// {"L_WRIST_R", -0.518287},
//
// {"R_SHOULDER_P", 0.226871},
// {"R_SHOULDER_R", 0.624717},
// {"R_SHOULDER_Y", 1.15086},
// {"R_ELBOW_R", 1.30365},
// {"R_WRIST_P", -2.14683},
// {"R_WRIST_Y", 0.196003},
// {"R_WRIST_R", 0.106678}
// };
//
//
//
// while (true) {
// robot->moveJ(cmd1,0.8);
// robot->moveJ(cmd2,0.8);
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
// }
auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01));
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
}
}
TEST(HumanoidRobotTest,MoveDeltaTest) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 0);
pose.mutable_position()->set_y(0);
pose.mutable_position()->set_z(0);
pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(-0.7854);
try {
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1);
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01);
robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.05);
}catch (std::exception &e) {
LOG(INFO) << e.what();
robot->torqueOff();
LOG(INFO) << e.what();
}
}
TEST(HumanoidRobotTest,AngleToTest) {
double rx = 0.9497;
double ry = 1.6082;
double rz = -2.4832;
Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);
std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
}
std::atomic<bool> running{true};
// 信号处理函数
void signalHandler(int signum) {
std::cout << "\nInterrupt signal (" << signum << ") received. Stopping..." << std::endl;
running = false;
}
TEST(HumanoidRobotTest, MoveIKTest) {
// 注册信号处理
std::signal(SIGINT, signalHandler);
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found";
return;
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP");
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x(0.047436);
pose.mutable_position()->set_y(-0.2884);
pose.mutable_position()->set_z(-0.605183);
pose.mutable_euler()->set_rx(-1.191976);
pose.mutable_euler()->set_ry(1.397317);
pose.mutable_euler()->set_rz(2.721208);
// Pose: {'x': 0.047436, 'y': -0.2884, 'z': -0.605183, 'rx': -1.191976, 'ry': 1.397317, 'rz': 2.721208}
// 打印关节状态线程
std::thread joint_state_thread([&]() {
while (running) {
auto joint_state = robot->getJointQ();
if (auto it = joint_state.find("WAIST_P"); it != joint_state.end()) {
LOG(INFO) << "WAIST_P POS = " << it->second;
} else {
LOG(WARNING) << "WAIST_P not found in joint_state";
}
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
});
// 动作控制线程
std::thread motion_thread([&]() {
while (running) {
robot->moveJ("PELVIS_S","R_FINGER_TIP",pose,0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
robot->moveJ("PELVIS_S","R_FINGER_TIP",cur_pose,0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
});
// 等待线程结束
if (motion_thread.joinable()) motion_thread.join();
if (joint_state_thread.joinable()) joint_state_thread.join();
LOG(INFO) << "Test finished gracefully.";
}
TEST(HumanoidRobotTest,MoveLTest) {
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
try {
// 定义基座和末端链路名（需与URDF模型中的链路名一致）
std::string base_link = "PELVIS_S"; // 基座坐标系
std::string ee_link = "R_WRIST_R_S"; // 末端执行器坐标系
// 定义目标位姿（直线运动的终点与当前的差值）
cmvr::msgs::Pose3d delta_pose;
delta_pose.mutable_position()->set_x(0.2);
delta_pose.mutable_position()->set_y(0);
delta_pose.mutable_position()->set_z(0);
delta_pose.mutable_euler()->set_rx(0);
delta_pose.mutable_euler()->set_ry(0);
delta_pose.mutable_euler()->set_rz(0);
// 4. 设置运动参数（速度单位：m/s，加速度单位：m/s²）
double vel = 0.1; // 最大线速度 0.1m/s
double acc = 0.05; // 加速度 0.05m/s²
// 调用moveL执行直线运动
robot->moveDeltaL(base_link, ee_link, delta_pose, vel, acc);
LOG(INFO) << "直线运动完成！";
} catch (const std::exception& e) {
// 捕获异常（如IK解算失败、状态非法等）
LOG(ERROR) << "moveL调用失败：" << e.what();
}
}
TEST(HumanoidRobotTest, SpeedJTest) {
// 配置文件路径
std::string config_path = "/home/tankaitao/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
// 检查配置文件是否包含 DeviceManager 节点
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found";
}
// 获取 DeviceManager 配置并初始化设备管理器
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
// 获取机器人实例
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("cam4");
ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr);
// 设置目标关节和相关参数
std::string joint_name = "L_WRIST_R"; // 目标关节名
RobotJointIndexDirection dir = RobotJointIndexDirection::Y_NEGATIVE; // 方向
double vel = -0.1; // 速度 (rad/s)
double acc = 1; // 加速度 (rad/s²)
try {
// 调用 speedJ 函数进行关节运动
robot->speedJ(joint_name, dir, vel, acc);
} catch (const std::exception& e) {
LOG(ERROR) << "speedJ test failed: " << e.what();
FAIL() << "Exception thrown during speedJ test: " << e.what();
}
}
TEST(HumanoidRobotTest, SpeedLTest) {
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
try {
// std::unordered_map<std::string,double> joint_qs;
// joint_qs.insert(std::make_pair("R_SHOULDER_P",0));
// joint_qs.insert(std::make_pair("R_SHOULDER_R",0));
// joint_qs.insert(std::make_pair("R_SHOULDER_Y",0));
// joint_qs.insert(std::make_pair("R_ELBOW_R",0));
// joint_qs.insert(std::make_pair("R_WRIST_P",0));
// joint_qs.insert(std::make_pair("R_WRIST_Y",0));
// joint_qs.insert(std::make_pair("R_WRIST_R",0));
//
// robot->getJointQ(joint_qs);
// LOG(INFO) << "======================================";
// LOG(INFO) << "获取到的关节位置信息 (单位: 弧度)";
// LOG(INFO) << "--------------------------------------";
//
// for (const auto& pair : joint_qs) {
// LOG(INFO) << "关节 " << pair.first
// << ": " << pair.second
// << " (" << pair.second * 180 / M_PI << "°)";
// }
// LOG(INFO) << "======================================";
// return ;
// 先运动到一个合适的位置
std::vector<JointPoint> cmd{
{"R_SHOULDER_P", -0.348663},
{"R_SHOULDER_R", 1.1512},
{"R_SHOULDER_Y", 1.64856},
{"R_ELBOW_R", 1.8978},
{"R_WRIST_P", -2.8002},
{"R_WRIST_Y", -0.0262979},
{"R_WRIST_R", 0.0449828}
};
robot->moveJ(cmd);
// 定义运动方向（笛卡尔坐标系）
RobotCartesian cart_direction = RobotCartesian::Y; // 沿X轴移动
// 定义运动方向（正向或反向）
RobotJointIndexDirection move_direction = RobotJointIndexDirection::ROTATE_Y; // 正向
// 设置运动参数（速度单位：m/s，加速度单位：m/s²）
double vel = 0.02; // 速度 0.1m/s
double acc = 0.05; // 加速度 0.05m/s²
LOG(INFO) << "开始执行速度控制，方向：X轴正向，速度：" << vel << "m/s，加速度：" << acc << "m/s²";
// 调用speedL执行速度控制
robot->speedL(cart_direction, move_direction, vel, acc);
// 等待一段时间让机器人运动
std::this_thread::sleep_for(std::chrono::seconds(5));
// 停止运动（假设有停止函数）
// robot->stopSpeedL();
LOG(INFO) << "速度控制完成！";
} catch (const std::exception& e) {
// 捕获异常（如IK解算失败、状态非法等）
LOG(ERROR) << "speedL调用失败：" << e.what();
}
}
TEST(HumanoidRobotTest, followPoseTest) {
// 1. 配置文件与设备初始化
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found in config file";
return; // 配置错误，直接退出测试
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
// 获取机器人设备（hc01）
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
if (!robot) {
LOG(ERROR) << "Failed to get robot device 'hc01'";
return;
}
try {
// 2. 核心参数配置
std::string base_link = "PELVIS_S"; // 基座坐标系（轨迹相对此坐标系）
std::string ee_link = "R_WRIST_R_S"; // 待控制的末端执行器（如右手腕）
const double circle_radius = 0.03; // 圆形轨迹半径（3厘米，根据工作空间调整）
const double angle_start = 0.0; // 轨迹起始角度（0弧度）
const double angle_end = 2 * M_PI; // 轨迹结束角度（2π，完整圆形）
const int num_points = 100; // 轨迹总点数（点数越多，轨迹越平滑）
const double dt = 0.02; // 相邻轨迹点的时间间隔（20ms，控制运动速度）
// 选择轨迹所在平面（保持某一轴坐标不变）
// 0: XY平面（Z轴固定） | 1: XZ平面（Y轴固定） | 2: YZ平面（X轴固定）
const int plane_choice = 2;
const std::string plane_desc = (plane_choice == 0) ? "XY平面（Z轴不变）" :
(plane_choice == 1) ? "XZ平面（Y轴不变）" : "YZ平面（X轴不变）";
LOG(INFO) << "Trajectory plane selected: " << plane_desc;
// 3. 获取末端执行器当前位姿（作为圆形轨迹的圆心）
LOG(INFO) << "Getting current pose of end-effector (" << ee_link << ") relative to base (" << base_link << ")";
const cmvr::math::Pose3d current_pose = robot->getTransform(base_link, ee_link);
// 将当前位姿转换为Eigen::Matrix4d（圆心位姿矩阵，包含位置和姿态）
Eigen::Matrix4d center_T = Eigen::Matrix4d::Identity(); // 初始化单位矩阵（姿态为默认，位置待填充）
// 3.1 填充圆心位置（从当前位姿的position提取）
center_T(0, 3) = current_pose.position.x; // 圆心X坐标（当前末端X）
center_T(1, 3) = current_pose.position.y; // 圆心Y坐标（当前末端Y）
center_T(2, 3) = current_pose.position.z; // 圆心Z坐标（当前末端Z）
LOG(INFO) << "Circle center pose (XYZ): ["
<< center_T(0,3) << ", " << center_T(1,3) << ", " << center_T(2,3) << "] (m)";
// 3.2 填充圆心姿态（从当前位姿的四元数转换为旋转矩阵）
const double w = current_pose.quaternion.w;
const double x = current_pose.quaternion.x;
const double y = current_pose.quaternion.y;
const double z = current_pose.quaternion.z;
// 四元数转旋转矩阵（标准公式，确保末端姿态与当前一致）
center_T.block<3,3>(0,0) << 1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w,
2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w,
2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y;
// 4. 生成指定平面内的圆形轨迹（格式：std::vector<std::vector<cmvr::ctrl::PoseTarget>>）
std::vector<std::vector<cmvr::ctrl::PoseTarget>> trajectory;
const double angle_step = (angle_end - angle_start) / num_points; // 每步角度增量
for (int i = 0; i <= num_points; ++i) {
const double current_angle = angle_start + i * angle_step; // 当前轨迹点角度
Eigen::Matrix4d point_T = center_T; // 复制圆心位姿，在此基础上偏移生成轨迹点
// 根据选择的平面，计算位置偏移（保持对应轴不变）
switch (plane_choice) {
case 0: // XY平面：Z轴固定，X/Y随角度变化
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
point_T(1, 3) += circle_radius * sin(current_angle); // Y方向偏移
break;
case 1: // XZ平面：Y轴固定，X/Z随角度变化
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
break;
case 2: // YZ平面：X轴固定，Y/Z随角度变化
point_T(1, 3) += circle_radius * cos(current_angle); // Y方向偏移
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
break;
default:
LOG(WARNING) << "Invalid plane choice (" << plane_choice << "), use default XY plane";
point_T(0, 3) += circle_radius * cos(current_angle);
point_T(1, 3) += circle_radius * sin(current_angle);
ASSERT_NO_THROW(camera->start());
struct RuntimeGuard {
std::shared_ptr<AbstractCamera> camera;
std::shared_ptr<AbstractRobot> robot;
~RuntimeGuard() {
if (camera) {
try {
camera->stop();
} catch (...) {
}
}
if (robot) {
try {
robot->eStop();
} catch (...) {
}
}
}
} guard{camera, robot};
// 构造当前时间步的PoseTarget（控制末端执行器）
cmvr::ctrl::PoseTarget ee_target;
ee_target.link_name = ee_link; // 目标控制的link（末端执行器）
ee_target.T_target = point_T; // 相对base_link的目标位姿矩阵
ee_target.w_posrot = 0.5; // 位置/姿态权重（0.5：位置和姿态同等重要）
ee_target.weight = 1.0; // 任务权重（1.0：硬约束，必须满足）
cmvr::IbvsController ibvs_controller;
ibvs_controller.setMu(0.1);
ibvs_controller.setQdotMax(0.6);
ibvs_controller.setTagSize(0.12);
ibvs_controller.setTrackedTagId(0);
ibvs_controller.setTarget(0.0, 0.0, 0.35, 3.14159265358979323846, 0.0, 0.0);
ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
ibvs_controller.setDepthZGain(1.0);
// 每个时间步可控制多个link（此处仅控制一个末端，故vector大小为1）
std::vector<cmvr::ctrl::PoseTarget> step_targets = {ee_target};
trajectory.push_back(step_targets); // 将当前时间步目标加入轨迹
ASSERT_TRUE(ibvs_controller.init(camera,
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S",
"R_WRIST_R_S",
"R_CAM"))
<< "IbvsController init failed";
auto q_map_now = makeRightArmQMap();
robot->getJointQ(q_map_now);
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
auto refreshRightArmQ = [&]() -> bool {
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
const auto it = q_map_now.find(kRightArmJointNames[i]);
if (it == q_map_now.end()) {
return false;
}
q_now[i] = it->second;
}
return true;
};
ASSERT_TRUE(refreshRightArmQ())
<< "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now);
const int max_steps = 300;
const int log_every = 10;
const int cycle_ms = 33;
int ok_steps = 0;
int fail_steps = 0;
auto t_prev = std::chrono::steady_clock::now();
for (int step = 0; step < max_steps; ++step) {
robot->getJointQ(q_map_now);
if (!refreshRightArmQ()) {
++fail_steps;
if ((step % log_every) == 0) {
std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n";
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
continue;
}
// 输出轨迹信息（调试用）
LOG(INFO) << "Trajectory generated successfully: "
<< trajectory.size() << " time steps, radius: " << circle_radius << "m";
const auto t_now = std::chrono::steady_clock::now();
double dt = std::chrono::duration<double>(t_now - t_prev).count();
t_prev = t_now;
dt = std::clamp(dt, 0.001, 0.1);
std::vector<double> q_cmd_next;
const bool ok = ibvs_controller.compute(q_now, dt, q_cmd_next);
if (!ok || q_cmd_next.size() != kRightArmJointNames.size()) {
++fail_steps;
if ((step % log_every) == 0) {
std::cout << "[IBVS_REAL] step=" << step
<< " compute failed: "
<< cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus())
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
continue;
}
// 5. 执行轨迹（调用followPoseTrajectory接口）
LOG(INFO) << "Starting circular trajectory execution...";
robot->followPoseTrajectory(base_link,trajectory, dt); // 传入轨迹和时间间隔
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8);
robot->servoJ(joint_cmd, 0.8, dt);
++ok_steps;
LOG(INFO) << "Circular trajectory execution completed!";
if ((step % log_every) == 0) {
const auto& t_co = ibvs_controller.lastTagPositionVisp();
const auto& v_c = ibvs_controller.lastCameraTwistVisp();
std::cout << "[IBVS_REAL] step=" << step
<< " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]"
<< " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2]
<< ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
<< " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
<< std::endl;
}
} catch (const std::exception& e) {
// 捕获并打印异常（如机器人未就绪、逆解失败等）
LOG(ERROR) << "Trajectory execution failed: " << e.what();
throw; // 可选：抛出异常让测试框架捕获，标记测试失败
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
}
std::cout << "[IBVS_REAL] finished steps=" << max_steps
<< " ok_steps=" << ok_steps
<< " fail_steps=" << fail_steps
<< std::endl;
EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
}

3
cmvr-es/ik_solver/CMakeLists.txt
View File

@ -21,9 +21,8 @@ target_link_libraries(ik_solver PUBLIC
)
add_library(cmvr_es::ik_solver ALIAS ik_solver)
install(TARGETS ik_solver RUNTIME DESTINATION bin)
install(TARGETS ik_solver LIBRARY DESTINATION lib)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------

30
cmvr-es/perception/CMakeLists.txt Normal file
View File

@ -0,0 +1,30 @@
find_package(VISP REQUIRED)
find_package(OpenCV REQUIRED)
add_library(perception SHARED
src/tag_relative_target_3d.cpp
)
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(perception PUBLIC
${VISP_LIBRARIES}
${OpenCV_LIBS}
)
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
)

215
cmvr-es/perception/include/tag_relative_target_3d.h Normal file
View File

@ -0,0 +1,215 @@
//
// Created by lgv on 2026/2/26.
//
#pragma once
#include <memory>
#include <unordered_map>
#include <vector>
#include <Eigen/Dense>
#include <visp3/detection/vpDetectorAprilTag.h>
#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 相对坐标”，并支持在线恢复目标点相机坐标。
*
* 典型流程：
* 1) 锁定目标时已知目标点相机坐标 `p_c_target` 与可见 tag 位姿观测；
* 2) 计算并缓存每个可见 tag 下的 `p_ti_target`；
* 3) 运行时只要某个已缓存的 tag 可见，就可恢复目标点在当前相机系下位置。
*/
class TagRelativeTarget3D {
public:
/**
* @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
};
/**
* @brief 构造函数，支持直接注入抽象相机。
* @param camera 抽象相机，可为空（后续可仅使用几何接口）。
*/
explicit TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
/**
* @brief 设置/替换相机对象。
* @param camera 抽象相机。
*/
void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) { camera_ = camera; }
/**
* @brief 清空全部 tag 锚点。
*/
void clear();
/**
* @brief 是否没有任何缓存锚点。
* @return 为空返回 `true`。
*/
bool empty() const { return anchors_in_tag_.empty(); }
/**
* @brief 当前缓存锚点数量。
* @return 锚点数量。
*/
size_t anchorCount() const { return anchors_in_tag_.size(); }
/**
* @brief 获取最近状态。
* @return 最近状态枚举。
*/
Status lastStatus() const { return last_status_; }
/**
* @brief 状态枚举转字符串。
* @param status 状态枚举。
* @return 字符串常量。
*/
static const char* statusToString(Status status);
/**
* @brief 设置 AprilTag 边长。
* @param tag_size_m tag 边长（米）。
*/
void setTagSize(double tag_size_m);
/**
* @brief 通过内部相机抓帧并检测 tag 观测（`T_c_t`）。
* @param observations 输出观测列表。
* @return 检测成功返回 `true`。
*/
bool detectObservations(std::vector<TagPoseObservation>& observations);
/**
* @brief 使用外部图像与内参检测 tag 观测（`T_c_t`）。
* @param color 输入彩色/灰度图。
* @param intrinsics 相机内参。
* @param observations 输出观测列表。
* @return 检测成功返回 `true`。
*/
bool detectObservationsFromImage(const cv::Mat& color,
const device::Rs2Intrinsics& intrinsics,
std::vector<TagPoseObservation>& observations);
/**
* @brief 使用内部相机检测结果直接建立/更新锚点。
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
* @param overwrite_existing 是否覆盖已存在锚点。
* @return 成功建立至少一个锚点返回 `true`。
*/
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target,
bool overwrite_existing = true);
/**
* @brief 使用内部相机检测结果直接恢复目标点相机坐标。
* @param p_c_target_out 输出目标点相机坐标（米）。
* @param used_tag_id_out 若非空，输出主导 tag id。
* @param spread_out 若非空，输出多 tag 融合离散误差（米）。
* @return 恢复成功返回 `true`。
*/
bool resolveTargetInCamera(Eigen::Vector3d& p_c_target_out,
int* used_tag_id_out = nullptr,
double* spread_out = nullptr);
/**
* @brief 根据“目标点相机坐标 + 可见 tag 观测”批量建立/更新各 tag 锚点。
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
* @param observations 可见 tag 观测集合。
* @param overwrite_existing 是否覆盖已存在的同 id 锚点。
* @return 成功建立至少一个锚点时返回 `true`。
*/
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target,
const std::vector<TagPoseObservation>& observations,
bool overwrite_existing = true);
/**
* @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<TagPoseObservation>& 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`。
*/
bool hasAnchorForTag(int tag_id) const;
/**
* @brief 获取目标点在指定 tag 坐标系下的 3D 相对坐标。
* @param tag_id tag id。
* @param p_t_target_out 输出 `p_t_target`（米）。
* @return 获取成功返回 `true`。
*/
bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const;
/**
* @brief 获取构造时注入的相机对象。
* @return 抽象相机智能指针。
*/
const std::shared_ptr<cmvr::device::AbstractCamera>& camera() const { return camera_; }
private:
static bool isFiniteMatrix(const Eigen::Matrix4d& T);
static bool isFiniteVector(const Eigen::Vector3d& p);
static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t);
static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c);
private:
// 缓存: tag_id -> 目标点在该 tag 坐标系下的 3D 坐标。
std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_;
// 抽象相机对象（由构造函数注入）。
std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
// AprilTag 检测器。
vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
// tag 边长（米）。
double tag_size_m_{0.12};
// 最近一次调用状态。
Status last_status_{Status::TARGET_NOT_LOCKED};
};
} // namespace cmvr::perception

356
cmvr-es/perception/src/tag_relative_target_3d.cpp Normal file
View File

@ -0,0 +1,356 @@
//
// Created by lgv on 2026/2/26.
//
#include "perception/include/tag_relative_target_3d.h"
#include <algorithm>
#include <cmath>
#include <cstring>
#include <limits>
#include <opencv2/imgproc.hpp>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
namespace cmvr::perception {
TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
: camera_(camera) {
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
}
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_TAG: return "no_tag";
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";
default: return "unknown";
}
}
void TagRelativeTarget3D::setTagSize(double tag_size_m) {
if (std::isfinite(tag_size_m) && tag_size_m > 0.0) {
tag_size_m_ = tag_size_m;
}
}
bool TagRelativeTarget3D::detectObservations(std::vector<TagPoseObservation>& observations) {
observations.clear();
if (!camera_) {
last_status_ = Status::NO_CAMERA;
return false;
}
cv::Mat color;
device::Rs2Intrinsics intrinsics{};
try {
// Tag 检测只需要彩色图像与相机内参，避免依赖深度流导致阻塞/超时。
camera_->getRGBImage(color, intrinsics);
} catch (const std::exception&) {
last_status_ = Status::NO_NEW_FRAME;
return false;
}
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<TagPoseObservation> observations;
if (!detectObservations(observations)) {
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<TagPoseObservation> observations;
if (!detectObservations(observations)) {
return false;
}
return resolveTargetInCamera(observations, p_c_target_out, used_tag_id_out, spread_out);
}
void TagRelativeTarget3D::clear() {
anchors_in_tag_.clear();
last_status_ = Status::TARGET_NOT_LOCKED;
}
bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target,
const std::vector<TagPoseObservation>& observations,
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::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<TagPoseObservation>& 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;
}
if (anchors_in_tag_.empty()) {
last_status_ = Status::TARGET_NOT_LOCKED;
return false;
}
if (observations.empty()) {
last_status_ = Status::NO_OBSERVATION;
return false;
}
std::vector<Eigen::Vector3d> matched_points;
matched_points.reserve(observations.size());
Eigen::Vector3d weighted_sum = Eigen::Vector3d::Zero();
double weight_sum = 0.0;
double best_weight = -std::numeric_limits<double>::infinity();
int best_id = -1;
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;
}
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;
}
}
if (matched_points.empty() || weight_sum <= 0.0) {
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_status_ = Status::OK;
return true;
}
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;
}
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;
}
}
}
return true;
}
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<TagPoseObservation>& 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<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 (!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;
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;
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<double>(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);
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::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);
}
} // namespace cmvr::perception

524
cmvr-es/perception/src/tag_relative_target_3d_test.cpp Normal file
View File

@ -0,0 +1,524 @@
//
// Created by lgv on 2026/2/26.
//
#include "perception/include/tag_relative_target_3d.h"
#include "devices/camera/realsense_camera/include/realsense_camera.h"
#include <gtest/gtest.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
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;
// <= 0 means loop until user abort.
constexpr int kTries = -1;
constexpr int kWidth = 1280;
constexpr int kHeight = 720;
constexpr int kFps = 30;
constexpr const char* kWindowName = "TagRelativeTarget3DRealSenseTest";
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;
struct FramePack {
cv::Mat color;
cv::Mat depth;
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<double> valid;
const int side = radius_px * 2 + 1;
valid.reserve(static_cast<size_t>(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<double>(depth.at<uint16_t>(vv, uu)) * 1e-3;
} else if (depth.type() == CV_32FC1) {
z = static_cast<double>(depth.at<float>(vv, uu));
} else {
return false;
}
if (std::isfinite(z) && z > 1e-4) {
valid.push_back(z);
}
}
}
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;
}
bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
FramePack& frame,
std::string& error_out) {
error_out.clear();
for (int retry = 0; retry < kFrameRetryMax; ++retry) {
try {
camera->getRGBDImages(frame.color, frame.depth, frame.intrinsics);
if (!frame.color.empty() && !frame.depth.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));
}
}
return false;
}
bool projectPoint(const Eigen::Vector3d& p_c,
const cmvr::device::Rs2Intrinsics& K,
cv::Point& uv) {
if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) {
return false;
}
if (p_c.z() <= 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) {
return false;
}
const double u = static_cast<double>(K.fx) * (p_c.x() / p_c.z()) + static_cast<double>(K.cx);
const double v = static_cast<double>(K.fy) * (p_c.y() / p_c.z()) + static_cast<double>(K.cy);
uv.x = static_cast<int>(std::lround(u));
uv.y = static_cast<int>(std::lround(v));
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) {
return false;
}
p_c.z() = z_m;
p_c.x() = (static_cast<double>(u) - static_cast<double>(K.cx)) / static_cast<double>(K.fx) * z_m;
p_c.y() = (static_cast<double>(v) - static_cast<double>(K.cy)) / static_cast<double>(K.fy) * z_m;
return true;
}
void drawTagAxes(cv::Mat& img,
const Eigen::Matrix4d& T_c_t,
const cmvr::device::Rs2Intrinsics& K,
double axis_len,
int tag_id) {
const Eigen::Matrix3d R = T_c_t.block<3,3>(0,0);
const Eigen::Vector3d t = T_c_t.block<3,1>(0,3);
const Eigen::Vector3d o_c = t;
const Eigen::Vector3d x_c = R * Eigen::Vector3d(axis_len, 0, 0) + t;
const Eigen::Vector3d y_c = R * Eigen::Vector3d(0, axis_len, 0) + t;
const Eigen::Vector3d z_c = R * Eigen::Vector3d(0, 0, axis_len) + t;
cv::Point o, x, y, z;
if (!projectPoint(o_c, K, o)) return;
if (projectPoint(x_c, K, x)) {
cv::line(img, o, x, cv::Scalar(0, 0, 255), 2); // X red
}
if (projectPoint(y_c, K, y)) {
cv::line(img, o, y, cv::Scalar(0, 255, 0), 2); // Y green
}
if (projectPoint(z_c, K, z)) {
cv::line(img, o, z, cv::Scalar(255, 0, 0), 2); // Z blue
}
cv::circle(img, o, 3, cv::Scalar(255, 255, 255), -1);
cv::putText(img,
"id=" + std::to_string(tag_id),
cv::Point(o.x + 6, o.y - 6),
cv::FONT_HERSHEY_SIMPLEX,
0.45,
cv::Scalar(255, 255, 255),
1);
}
} // namespace
TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
const std::string serial = kRsSerial;
if (serial.empty()) {
GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
}
const double tag_size = kTagSize;
const int tries = kTries;
const bool infinite = (tries <= 0);
const int width = kWidth;
const int height = kHeight;
const int fps = kFps;
std::cout << "[TagRelativeTarget3DTest] serial=" << serial
<< " target_uv=[" << kTargetU << ", " << kTargetV << "]"
<< " tag_size=" << tag_size
<< " tries=" << tries << "\n";
cmvr::config::RealSenseCameraConfig cam_cfg;
cam_cfg.set_id("tag_relative_target_3d_test");
cam_cfg.set_serialnumber(serial);
cam_cfg.set_width(width);
cam_cfg.set_height(height);
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_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);
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};
bool window_enabled = true;
try {
cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
cv::resizeWindow(kWindowName, width, height);
cv::imshow(kWindowName, cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)));
cv::waitKey(1);
} catch (const cv::Exception& e) {
window_enabled = false;
std::cout << "[TagRelativeTarget3DTest] window disabled: " << e.what() << "\n";
}
if (!window_enabled) {
GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
}
bool ok = false;
bool user_abort = false;
int frame_fail_count = 0;
int detect_success_count = 0;
int target_valid_count = 0;
int lock_success_count = 0;
int printed_count = 0;
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.
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);
if (warmup_ok) {
break;
}
const int key = cv::waitKey(1);
if (key == 27 || key == 'q' || key == 'Q') {
warmup_abort = true;
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs));
}
if (warmup_abort) {
GTEST_SKIP() << "User aborted during warmup";
}
if (!warmup_ok) {
std::cout << "[TagRelativeTarget3DTest] warmup failed, continue running and retry per frame\n";
}
for (int i = 0; infinite || i < tries; ++i) {
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);
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<cmvr::perception::TagPoseObservation> 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;
}
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<double>(target_u) - static_cast<double>(frame.intrinsics.cx))
/ static_cast<double>(frame.intrinsics.fx) * z_depth_m;
p_c_target.y() = (static_cast<double>(target_v) - static_cast<double>(frame.intrinsics.cy))
/ static_cast<double>(frame.intrinsics.fy) * z_depth_m;
target_pc_valid = true;
++target_valid_count;
}
}
cv::Mat vis = frame.color.clone();
cv::drawMarker(vis,
cv::Point(target_u, target_v),
cv::Scalar(0, 255, 255),
cv::MARKER_CROSS,
16,
2);
cv::putText(vis,
std::string("status: ") + status_str,
cv::Point(12, 28),
cv::FONT_HERSHEY_SIMPLEX,
0.7,
cv::Scalar(0, 255, 0),
2);
cv::putText(vis,
"press q / Esc to quit",
cv::Point(12, 56),
cv::FONT_HERSHEY_SIMPLEX,
0.6,
cv::Scalar(0, 255, 255),
2);
cv::putText(vis,
"tags=" + std::to_string(observations.size()) +
" frame_fail=" + std::to_string(frame_fail_count),
cv::Point(12, 84),
cv::FONT_HERSHEY_SIMPLEX,
0.5,
cv::Scalar(200, 200, 255),
1);
if (!frame_error.empty()) {
cv::putText(vis,
"frame error: " + frame_error,
cv::Point(12, 106),
cv::FONT_HERSHEY_SIMPLEX,
0.45,
cv::Scalar(0, 0, 255),
1);
}
cv::putText(vis,
std::string("depth@target: ") + (target_pc_valid ? std::to_string(z_depth_m) + "m" : "invalid"),
cv::Point(12, 128),
cv::FONT_HERSHEY_SIMPLEX,
0.5,
target_pc_valid ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255),
1);
// Draw detected tag axes (camera frame).
if (detected && !observations.empty()) {
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);
}
}
// Draw target point (projected from camera coordinates).
if (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;
const double v = frame.intrinsics.fy * (p_c_target.y() / p_c_target.z()) + frame.intrinsics.cy;
const int ui = static_cast<int>(std::lround(u));
const int vi = static_cast<int>(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::putText(vis,
"target",
cv::Point(ui + 8, vi - 8),
cv::FONT_HERSHEY_SIMPLEX,
0.5,
cv::Scalar(0, 0, 255),
1);
}
}
try {
cv::imshow(kWindowName, vis);
const int key = cv::waitKey(1);
if (key == 27 || key == 'q' || key == 'Q') {
user_abort = true;
break;
}
} catch (const cv::Exception& e) {
std::cout << "[TagRelativeTarget3DTest] window error: " << e.what() << "\n";
user_abort = true;
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;
}
try {
cv::destroyWindow(kWindowName);
} catch (...) {
// ignore window destroy errors in headless environments
}
if (!user_abort && !infinite) {
EXPECT_TRUE(ok) << "No valid tag detected in " << tries << " tries";
}
const auto t_loop_end = std::chrono::steady_clock::now();
const double total_s = std::chrono::duration_cast<std::chrono::duration<double>>(t_loop_end - t_loop_start).count();
std::cout << "[TagRelativeTarget3DTest] summary "
<< "detect_ok=" << detect_success_count
<< " target_valid=" << target_valid_count
<< " lock_ok=" << lock_success_count
<< " printed=" << printed_count
<< " frame_fail=" << frame_fail_count
<< " duration_s=" << total_s
<< "\n";
}

BIN
dependency/x86/third_party/opencv/4.13.0/bin/opencv_annotation vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/bin/opencv_interactive-calibration vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/bin/opencv_model_diagnostics vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/bin/opencv_version vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/bin/opencv_visualisation vendored
View File

Binary file not shown.

2
dependency/x86/third_party/opencv/4.13.0/include/opencv4/opencv2/cvconfig.h vendored
View File

@ -60,7 +60,7 @@
/* #undef HAVE_VULKAN */
/* Define to 1 if you have the <inttypes.h> header file. */
#define HAVE_INTTYPES_H 1
/* #undef HAVE_INTTYPES_H */
/* Intel Integrated Performance Primitives */
#define HAVE_IPP

1
dependency/x86/third_party/opencv/4.13.0/lib/cmake/opencv4/OpenCVModules-release.cmake vendored
View File

@ -78,6 +78,7 @@ list(APPEND _IMPORT_CHECK_FILES_FOR_opencv_features2d "${_IMPORT_PREFIX}/lib/lib
# Import target "opencv_imgcodecs" for configuration "Release"
set_property(TARGET opencv_imgcodecs APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE)
set_target_properties(opencv_imgcodecs PROPERTIES
IMPORTED_LINK_DEPENDENT_LIBRARIES_RELEASE "openjp2"
IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libopencv_imgcodecs.so.4.13.0"
IMPORTED_SONAME_RELEASE "libopencv_imgcodecs.so.413"
)

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_calib3d.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_core.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_dnn.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_features2d.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_flann.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_gapi.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_highgui.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgcodecs.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_imgproc.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_ml.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_objdetect.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_photo.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_stitching.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_video.so.4.13.0 vendored
View File

Binary file not shown.

BIN
dependency/x86/third_party/opencv/4.13.0/lib/libopencv_videoio.so.4.13.0 vendored
View File

Binary file not shown.

13
dependency/x86/third_party/opencv/4.13.0/lib/pkgconfig/opencv4.pc vendored Normal file
View File

@ -0,0 +1,13 @@
# Package Information for pkg-config
prefix=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/opencv/4.13.0
exec_prefix=${prefix}
libdir=${exec_prefix}/lib
includedir=${prefix}/include/opencv4
Name: OpenCV
Description: Open Source Computer Vision Library
Version: 4.13.0
Libs: -L${exec_prefix}/lib -lopencv_gapi -lopencv_highgui -lopencv_ml -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_video -lopencv_calib3d -lopencv_features2d -lopencv_dnn -lopencv_flann -lopencv_videoio -lopencv_imgcodecs -lopencv_imgproc -lopencv_core
Libs.private: -ldl -lm -lpthread -lrt
Cflags: -I${includedir}