13.rk3576环境下机器人视觉——姿态获取
·
rk3576的机器人视觉在这里用的是传统算法求6d位姿，二维分割用了yolo，所以需要将分割算法的pt模型转为rknn模型。整个算法环境和模型转换步骤在后面详细说。以下环境没有单独建立conda环境，所有环境都安装在系统环境中（没有建立独立的conda环境）。
1.在rk3576板子上验证是否启用gpu
该节内容在rk3576板子上操作。
1.检查设备树是否启用NPU
# 查看设备树中的NPU节点
cat /proc/device-tree/npu/status 2>/dev/null
# 或者
find /proc/device-tree -name "npu" -exec cat {}/status \; 2>/dev/null
# 查看完整的设备树
ls -la /proc/device-tree/
2.检查NPU节点的状态
# 检查NPU节点状态
cat /proc/device-tree/npu@27700000/status 2>/dev/null
# 查看NPU节点的内容
ls -la /proc/device-tree/npu@27700000/
# 查看内核日志中NPU初始化情况
dmesg | grep -i "npu\|rknpu" | tail -20
# 检查是否有NPU相关错误
dmesg | grep -i "rknpu" -A 5 -B 5
3.验证rknn模型
pip3 install rknn-toolkit-lite2 --user
cd /usr/share/RK3576
sudo /usr/bin/rknn_common_test --model mobilenet_v1.rknn --input ../dog_224x224.jpg
2.pt模型转为rknn模型
1.pt模型转换为onnx模型
在这里用yolov8n_seg.pt的分割模型为例转换模型。首先在pc端（不是在板子上，在电脑上）下再yolov8的开源代码，代码中有个export.py文件，这个文件就是将pt文件转换为onnx模型的文件，export.py代码在test文件夹下面。
代码如下：
# Ultralytics 🚀 AGPL-3.0 License - https://ultralytics.com/license
import shutil
import uuid
from itertools import product
from pathlib import Path
import pytest
from tests import MODEL, SOURCE
from ultralytics import YOLO
from ultralytics.cfg import TASK2DATA, TASK2MODEL, TASKS
from ultralytics.utils import (
ARM64,
IS_RASPBERRYPI,
LINUX,
MACOS,
WINDOWS,
checks,
)
from ultralytics.utils.torch_utils import TORCH_1_9, TORCH_1_13
def test_export_torchscript():
"""Test YOLO model exporting to TorchScript format for compatibility and correctness."""
file = YOLO(MODEL).export(format="torchscript", optimize=False, imgsz=32)
YOLO(file)(SOURCE, imgsz=32) # exported model inference
def test_export_onnx():
"""Test YOLO model export to ONNX format with dynamic axes."""
file = YOLO(MODEL).export(format="onnx", dynamic=True, imgsz=32)#false
YOLO(file)(SOURCE, imgsz=32) # exported model inference
@pytest.mark.skipif(not TORCH_1_13, reason="OpenVINO requires torch>=1.13")
def test_export_openvino():
"""Test YOLO exports to OpenVINO format for model inference compatibility."""
file = YOLO(MODEL).export(format="openvino", imgsz=32)
YOLO(file)(SOURCE, imgsz=32) # exported model inference
@pytest.mark.slow
@pytest.mark.skipif(not TORCH_1_13, reason="OpenVINO requires torch>=1.13")
@pytest.mark.parametrize(
"task, dynamic, int8, half, batch, nms",
[ # generate all combinations except for exclusion cases
(task, dynamic, int8, half, batch, nms)
for task, dynamic, int8, half, batch, nms in product(
TASKS, [True, False], [True, False], [True, False], [1, 2], [True, False]
)
if not ((int8 and half) or (task == "classify" and nms))
],
)
def test_export_openvino_matrix(task, dynamic, int8, half, batch, nms):
"""Test YOLO model exports to OpenVINO under various configuration matrix conditions."""
file = YOLO(TASK2MODEL[task]).export(
format="openvino",
imgsz=32,
dynamic=dynamic,
int8=int8,
half=half,
batch=batch,
data=TASK2DATA[task],
nms=nms,
)
if WINDOWS:
# Use unique filenames due to Windows file permissions bug possibly due to latent threaded use
# See https://github.com/ultralytics/ultralytics/actions/runs/8957949304/job/24601616830?pr=10423
file = Path(file)
file = file.rename(file.with_stem(f"{file.stem}-{uuid.uuid4()}"))
YOLO(file)([SOURCE] * batch, imgsz=64 if dynamic else 32) # exported model inference
shutil.rmtree(file, ignore_errors=True) # retry in case of potential lingering multi-threaded file usage errors
@pytest.mark.slow
@pytest.mark.parametrize(
"task, dynamic, int8, half, batch, simplify, nms",
[ # generate all combinations except for exclusion cases
(task, dynamic, int8, half, batch, simplify, nms)
for task, dynamic, int8, half, batch, simplify, nms in product(
TASKS, [True, False], [False], [False], [1, 2], [True, False], [True, False]
)
if not ((int8 and half) or (task == "classify" and nms) or (task == "obb" and nms and not TORCH_1_13))
],
)
def test_export_onnx_matrix(task, dynamic, int8, half, batch, simplify, nms):
"""Test YOLO exports to ONNX format with various configurations and parameters."""
file = YOLO(TASK2MODEL[task]).export(
format="onnx", imgsz=32, dynamic=dynamic, int8=int8, half=half, batch=batch, simplify=simplify, nms=nms
)
YOLO(file)([SOURCE] * batch, imgsz=64 if dynamic else 32) # exported model inference
Path(file).unlink() # cleanup
@pytest.mark.slow
@pytest.mark.parametrize(
"task, dynamic, int8, half, batch, nms",
[ # generate all combinations except for exclusion cases
(task, dynamic, int8, half, batch, nms)
for task, dynamic, int8, half, batch, nms in product(TASKS, [False], [False], [False], [1, 2], [True, False])
if not (task == "classify" and nms)
],
)
def test_export_torchscript_matrix(task, dynamic, int8, half, batch, nms):
"""Tests YOLO model exports to TorchScript format under varied configurations."""
file = YOLO(TASK2MODEL[task]).export(
format="torchscript", imgsz=32, dynamic=dynamic, int8=int8, half=half, batch=batch, nms=nms
)
YOLO(file)([SOURCE] * batch, imgsz=64 if dynamic else 32) # exported model inference
Path(file).unlink() # cleanup
@pytest.mark.slow
@pytest.mark.skipif(not MACOS, reason="CoreML inference only supported on macOS")
@pytest.mark.skipif(not TORCH_1_9, reason="CoreML>=7.2 not supported with PyTorch<=1.8")
@pytest.mark.skipif(checks.IS_PYTHON_3_12, reason="CoreML not supported in Python 3.12")
@pytest.mark.parametrize(
"task, dynamic, int8, half, batch, nms",
[ # generate all combinations except for exclusion cases
(task, dynamic, int8, half, batch, nms)
for task, dynamic, int8, half, batch, nms in product(
TASKS, [False], [True, False], [True, False], [1], [True, False]
)
if not ((int8 and half) or (task == "classify" and nms))
],
)
def test_export_coreml_matrix(task, dynamic, int8, half, batch, nms):
"""Test YOLO exports to CoreML format with various parameter configurations."""
file = YOLO(TASK2MODEL[task]).export(
format="coreml",
imgsz=32,
dynamic=dynamic,
int8=int8,
half=half,
batch=batch,
nms=nms,
)
YOLO(file)([SOURCE] * batch, imgsz=32) # exported model inference at batch=3
shutil.rmtree(file) # cleanup
@pytest.mark.slow
@pytest.mark.skipif(not checks.IS_PYTHON_MINIMUM_3_10, reason="TFLite export requires Python>=3.10")
@pytest.mark.skipif(not LINUX, reason="Test disabled as TF suffers from install conflicts on Windows and macOS")
@pytest.mark.parametrize(
"task, dynamic, int8, half, batch, nms",
[ # generate all combinations except for exclusion cases
(task, dynamic, int8, half, batch, nms)
for task, dynamic, int8, half, batch, nms in product(
TASKS, [False], [True, False], [True, False], [1], [True, False]
)
if not ((int8 and half) or (task == "classify" and nms))
],
)
def test_export_tflite_matrix(task, dynamic, int8, half, batch, nms):
"""Test YOLO exports to TFLite format considering various export configurations."""
file = YOLO(TASK2MODEL[task]).export(
format="tflite", imgsz=32, dynamic=dynamic, int8=int8, half=half, batch=batch, nms=nms
)
YOLO(file)([SOURCE] * batch, imgsz=32) # exported model inference at batch=3
Path(file).unlink() # cleanup
@pytest.mark.skipif(not TORCH_1_9, reason="CoreML>=7.2 not supported with PyTorch<=1.8")
@pytest.mark.skipif(WINDOWS, reason="CoreML not supported on Windows") # RuntimeError: BlobWriter not loaded
@pytest.mark.skipif(LINUX and ARM64, reason="CoreML not supported on aarch64 Linux")
@pytest.mark.skipif(checks.IS_PYTHON_3_12, reason="CoreML not supported in Python 3.12")
def test_export_coreml():
"""Test YOLO exports to CoreML format, optimized for macOS only."""
if MACOS:
file = YOLO(MODEL).export(format="coreml", imgsz=32)
YOLO(file)(SOURCE, imgsz=32) # model prediction only supported on macOS for nms=False models
else:
YOLO(MODEL).export(format="coreml", nms=True, imgsz=32)
@pytest.mark.skipif(not checks.IS_PYTHON_MINIMUM_3_10, reason="TFLite export requires Python>=3.10")
@pytest.mark.skipif(not LINUX, reason="Test disabled as TF suffers from install conflicts on Windows and macOS")
def test_export_tflite():
"""Test YOLO exports to TFLite format under specific OS and Python version conditions."""
model = YOLO(MODEL)
file = model.export(format="tflite", imgsz=32)
YOLO(file)(SOURCE, imgsz=32)
@pytest.mark.skipif(True, reason="Test disabled")
@pytest.mark.skipif(not LINUX, reason="TF suffers from install conflicts on Windows and macOS")
def test_export_pb():
"""Test YOLO exports to TensorFlow's Protobuf (*.pb) format."""
model = YOLO(MODEL)
file = model.export(format="pb", imgsz=32)
YOLO(file)(SOURCE, imgsz=32)
@pytest.mark.skipif(True, reason="Test disabled as Paddle protobuf and ONNX protobuf requirements conflict.")
def test_export_paddle():
"""Test YOLO exports to Paddle format, noting protobuf conflicts with ONNX."""
YOLO(MODEL).export(format="paddle", imgsz=32)
@pytest.mark.slow
@pytest.mark.skipif(IS_RASPBERRYPI, reason="MNN not supported on Raspberry Pi")
def test_export_mnn():
"""Test YOLO exports to MNN format (WARNING: MNN test must precede NCNN test or CI error on Windows)."""
file = YOLO(MODEL).export(format="mnn", imgsz=32)
YOLO(file)(SOURCE, imgsz=32) # exported model inference
@pytest.mark.slow
def test_export_ncnn():
"""Test YOLO exports to NCNN format."""
file = YOLO(MODEL).export(format="ncnn", imgsz=32)
YOLO(file)(SOURCE, imgsz=32) # exported model inference
@pytest.mark.skipif(True, reason="Test disabled as keras and tensorflow version conflicts with tflite export.")
@pytest.mark.skipif(not LINUX or MACOS, reason="Skipping test on Windows and Macos")
def test_export_imx():
"""Test YOLO exports to IMX format."""
model = YOLO("yolov8n.pt")
file = model.export(format="imx", imgsz=32)
YOLO(file)(SOURCE, imgsz=32)
终端运行该代码如下：
yolo export model=yolov8n-seg.pt format=onnx imgsz=640 opset=17 dynamic=False simplify=True
得到了rknn模型：
2.onnx模型转为rknn模型
首先下载rknn_model_zoo-main.zip，解压缩该文件后进入下面路径：
该路径下就是yolov8_seg模型的转换，
运行转换代码如下：
from rknn.api import RKNN
print("="*50)
print("YOLOv8-seg RKNN 转换 (FP32 不量化)")
print("="*50)
rknn = RKNN(verbose=True)
# 配置 - 不量化
rknn.config(
mean_values=[[0, 0, 0]],
std_values=[[255, 255, 255]],
target_platform='rk3576'
)
print("\n加载 ONNX...")
ret = rknn.load_onnx(model='yolov8n-seg.onnx')
if ret != 0:
print(f"加载失败: {ret}")
exit(ret)
print("✅ 加载成功")
print("\n构建 RKNN 模型 (不量化)...")
ret = rknn.build(do_quantization=False)
if ret != 0:
print(f"构建失败: {ret}")
exit(ret)
print("✅ 构建成功")
print("\n导出 RKNN...")
ret = rknn.export_rknn('yolov8n_seg_fp32.rknn')
if ret != 0:
print(f"导出失败: {ret}")
exit(ret)
print("✅ 导出成功")
print("\n" + "="*50)
print("✅ 转换完成！模型: yolov8n_seg_fp32.rknn")
print("="*50)
rknn.release()
最后转换得到rknn模型yolov8n_seg_fp32.rknn
再将该模型复制到板子的项目中。
3,简单的模型预测
代码如下：
import cv2
import numpy as np
import time
from rknnlite.api import RKNNLite
CLASSES = ['person','bicycle','car','motorcycle','airplane','bus','train','truck',
'boat','traffic light','fire hydrant','stop sign','parking meter','bench',
'bird','cat','dog','horse','sheep','cow','elephant','bear','zebra',
'giraffe','backpack','umbrella','handbag','tie','suitcase','frisbee',
'skis','snowboard','sports ball','kite','baseball bat','baseball glove',
'skateboard','surfboard','tennis racket','bottle','wine glass','cup',
'fork','knife','spoon','bowl','banana','apple','sandwich','orange',
'broccoli','carrot','hot dog','pizza','donut','cake','chair','couch',
'potted plant','bed','dining table','toilet','tv','laptop','mouse',
'remote','keyboard','cell phone','microwave','oven','toaster','sink',
'refrigerator','book','clock','vase','scissors','teddy bear','hair drier','toothbrush']
def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))
def xywh2xyxy(x):
y = np.zeros_like(x)
y[:,0] = x[:,0] - x[:,2] / 2
y[:,1] = x[:,1] - x[:,3] / 2
y[:,2] = x[:,0] + x[:,2] / 2
y[:,3] = x[:,1] + x[:,3] / 2
return y
def nms_cv2(boxes, scores, conf_thr, iou_thr):
if len(boxes) == 0:
return np.array([])
boxes_list = boxes.tolist()
scores_list = scores.tolist()
idx = cv2.dnn.NMSBoxes(boxes_list, scores_list, conf_thr, iou_thr)
return idx.flatten()
def main():
# 使用正常的FP32模型
MODEL_PATH = "yolov8n_seg_fp32.rknn"
IMG_PATH = "bus.jpg"
CONF_THRESH = 0.6
IOU_THRESH = 0.45
INPUT_SIZE = (640,640)
W, H = INPUT_SIZE
print("Loading FP32 RKNN model (fully functional)...")
rknn = RKNNLite()
ret = rknn.load_rknn(MODEL_PATH)
if ret != 0:
print("RKNN load failed!")
return
ret = rknn.init_runtime()
if ret != 0:
print("RKNN init failed!")
rknn.release()
return
start = time.time()
img_origin = cv2.imread(IMG_PATH)
if img_origin is None:
print(f"Read image {IMG_PATH} failed")
rknn.release()
return
h_ori, w_ori = img_origin.shape[:2]
img_resize = cv2.resize(img_origin, INPUT_SIZE)
img_rgb = cv2.cvtColor(img_resize, cv2.COLOR_BGR2RGB)
input_data = np.expand_dims(img_rgb, axis=0).astype(np.uint8)
t0 = time.time()
outs = rknn.inference([input_data])
t1 = time.time()
print(f"Inference time: {int((t1-t0)*1000)}ms")
# FP32模型无需反量化，直接读取浮点输出
det_out = outs[0][0].T
proto = outs[1][0]
xywh_all = det_out[:, :4]
cls_logits = det_out[:, 4:84]
mask_coeff_all = det_out[:, 84:116]
conf_all = np.max(sigmoid(cls_logits), axis=1)
cls_id_all = np.argmax(cls_logits, axis=1)
valid_mask = conf_all > CONF_THRESH
xywh_valid = xywh_all[valid_mask]
conf_valid = conf_all[valid_mask]
clsid_valid = cls_id_all[valid_mask]
mask_coeff_valid = mask_coeff_all[valid_mask]
print(f"After confidence ({CONF_THRESH}): {len(xywh_valid)} boxes")
if len(xywh_valid) == 0:
print("No objects detected")
cv2.imwrite("result_fp32_final.jpg", img_origin)
rknn.release()
return
xyxy_valid = xywh2xyxy(xywh_valid)
# 过滤超出图像边界的无效框
valid_box_mask = ~((xyxy_valid[:,0] >= W) | (xyxy_valid[:,1] >= H) | (xyxy_valid[:,2] <= 0) | (xyxy_valid[:,3] <= 0))
xyxy_valid = xyxy_valid[valid_box_mask]
conf_valid = conf_valid[valid_box_mask]
clsid_valid = clsid_valid[valid_box_mask]
mask_coeff_valid = mask_coeff_valid[valid_box_mask]
nms_idx = nms_cv2(xyxy_valid, conf_valid, CONF_THRESH, IOU_THRESH)
print(f"After NMS & border filter: {len(nms_idx)} objects")
final_xyxy = xyxy_valid[nms_idx]
final_conf = conf_valid[nms_idx]
final_clsid = clsid_valid[nms_idx]
final_mask_coeff = mask_coeff_valid[nms_idx]
draw_img = img_resize.copy()
colors = np.random.randint(0,255,size=(80,3),dtype=np.uint8)
for i in range(len(final_xyxy)):
x1,y1,x2,y2 = final_xyxy[i].astype(int)
# 裁剪坐标防止越界
x1 = np.clip(x1, 0, W-1)
y1 = np.clip(y1, 0, H-1)
x2 = np.clip(x2, 0, W-1)
y2 = np.clip(y2, 0, H-1)
cid = final_clsid[i]
score = final_conf[i]
color = colors[cid]
label = f"{CLASSES[cid]} {score:.2f}"
cv2.rectangle(draw_img, (x1,y1), (x2,y2), color.tolist(), 2)
cv2.putText(draw_img, label, (x1, y1-6), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color.tolist(), 2)
coeff = final_mask_coeff[i]
mask_pred = np.matmul(coeff, proto.reshape(32, -1)).reshape(160,160)
mask_pred = sigmoid(mask_pred)
mask_bin = (mask_pred > 0.5).astype(np.uint8)
# 线性插值平滑掩码，消除锯齿
mask_resized = cv2.resize(mask_bin, INPUT_SIZE, interpolation=cv2.INTER_LINEAR)
mask_area = mask_resized == 1
# 只保留检测框内部掩码，框外全部清零，消除边缘杂色
box_mask = np.zeros_like(mask_resized)
box_mask[y1:y2, x1:x2] = 1
mask_area = np.logical_and(mask_area, box_mask)
color_fill = np.zeros_like(draw_img)
color_fill[:] = color
# 降低掩码透明度，原图更清晰
draw_img[mask_area] = cv2.addWeighted(draw_img[mask_area], 0.7, color_fill[mask_area], 0.3, 0)
out_img = cv2.resize(draw_img, (w_ori, h_ori))
end = time.time()
cv2.imwrite("result_fp32_final.jpg", out_img)
print("Saved fixed FP32 result: result_fp32_final.jpg")
print("总耗时：", end - start)
rknn.release()
if __name__ == "__main__":
main()
3.ros2环境安装
1. 安装 pip
sudo apt install python3-pip -y
2.安装 ROS2 Humble
#更新系统 & 开启 universe 源
sudo apt update && sudo apt upgrade -y
sudo apt install software-properties-common locales curl gnupg lsb-release -y
sudo apt update
sudo apt install software-properties-common -y
3..配置 UTF-8 语言
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
4.配置清华 ROS2 源
sudo curl -sSL https://mirrors.tuna.tsinghua.edu.cn/rosdistro/ros.key | gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] https://mirrors.tuna.tsinghua.edu.cn/ros2/ubuntu jammy main" | sudo tee /etc/apt/sources.list.d/ros2.list
sudo apt update
5..安装ROS2（4G + 内存，带 rviz2/turtlesim/rqt）
sudo apt install ros-humble-desktop -y
6.安装编译工具 & rosdep 依赖管理器
# colcon编译工具、rosdep
sudo apt install python3-colcon-common-extensions python3-rosdep python3-argcomplete -y
7.永久配置环境变量
# 写入bashrc，开机自动加载ROS
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc
# 验证环境变量
echo $ROS_DISTRO
# 输出 humble 代表配置成功
8.基础通信测试（验证安装正常）
#终端 1（发布话题）
ros2 run demo_nodes_cpp talker
#终端 2（订阅话题）
ros2 run demo_nodes_py listener
9.替换 ubuntu 系统清华源
sudo tee /etc/apt/sources.list <<'EOF'
deb [arch=arm64] https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ jammy main restricted universe multiverse
deb [arch=arm64] https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ jammy-updates main restricted universe multiverse
deb [arch=arm64] https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ jammy-backports main restricted universe multiverse
deb [arch=arm64] https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ jammy-security main restricted universe multiverse
EOF
10.创建ROS 工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build --symlink-install
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
11.安装ros2其他依赖包
sudo apt install -y \
ros-humble-rclpy \
ros-humble-action-msgs \
ros-humble-rcl-action \
ros-humble-geometry-msgs \
ros-humble-sensor-msgs \
ros-humble-std-msgs \
ros-humble-cv-bridge \
ros-humble-image-transport \
ros-humble-vision-msgs \
python3-opencv \
python3-colcon-common-extensions\
sudo apt update\
sudo apt install ros-humble-backward-ros
4.gemini相机驱动
1.克隆官方源码
cd ~/ros2_ws/src
git clone -b v2-main https://gitee.com/orbbecdeveloper/OrbbecSDK_ROS2.git
2.安装编译依赖
sudo apt install -y libgflags-dev nlohmann-json3-dev libgoogle-glog-dev \
ros-humble-image-transport ros-humble-image-publisher ros-humble-camera-info-manager \
ros-humble-diagnostic-updater ros-humble-diagnostic-msgs ros-humble-xacro
3.编译、设置环境
cd ~/ros2_ws
# 1. 先只编译相机包（安全）
colcon build --packages-select orbbec_camera orbbec_camera_msgs orbbec_description --symlink-install --event-handlers console_direct+
# 2. 刷新环境
source ~/ros2_ws/install/setup.bash
# 3. 测试相机是否工作
ros2 launch orbbec_camera gemini_330_series.launch.py
# 4.加载环境（加入~/.bashrc可永久生效）
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
4.固件升级（在windowns系统下操作）
#1. 下载Gemini330_Release_1.8.10
进入网址https://doc.orbbec.com/documentation/Orbbec%20Gemini%20330%20Series%20Documentation/Firmware%20Release%20(Gemini%20330%20Series)?_gl=1 下载
#2.下载OrbbecViewer_v2.7.6_202602022045_20730ef_win_x64.zip文件
进入网址https://gitee.com/orbbecdeveloper/OrbbecSDK_v2/releases#release-v2.8.7下载
#3.下载完以上两个文件后，解压，然后进入文件夹OrbbecViewer_v2.7.6_202602022045_20730ef_win_x64，点击OrbbecViewer.exe文件，将相机连接到windowns的电脑，根据提示升级固件即可。
#4.固件升级完成后，再将相机连接到rk3576开发板上打开相机。
5.打开相机，深度图和彩图对齐
ros2 launch orbbec_camera gemini_330_series.launch.py depth_registration:=true interleave_ae_mode:="off"
5.视觉ROS 融合额外安装包
#1 板子同时跑 RKNN 推理，需要视觉消息包
sudo apt install ros-humble-image-transport ros-humble-cv-bridge ros-humble-sensor-msgs ros-humble-vision-msgs -y
#2 pip 额外依赖（代码用到数值、点云、RKNN）
pip3 install numpy scipy open3d
#3 系统底层图形依赖
sudo apt install -y libgl1-mesa-glx libeigen3-dev libopenblas-dev build-essential cmake
6.rknn模型检测目标物体的6d位姿
代码如下：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from cv_bridge import CvBridge
from std_msgs.msg import Header
import cv2
import numpy as np
import struct
import time
#from ultralytics import YOLO
from rknnlite.api import RKNNLite
import open3d as o3d
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
#from vision_detection_action.action import VisionDetection
from com_interfaces.action import VisionDetection
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.qos import QoSProfile
import threading
import copy
import os
import itertools
import math
from typing import Tuple
# 屏蔽无关警告
os.environ["QT_LOGGING_RULES"] = "qt.fonts.warning=false"
os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
os.environ["CV_LOG_LEVEL"] = "FATAL"
# ============================================================================
# 📋 可调参数配置区域 - 请根据实际需求修改
# ============================================================================
# # -------------------- YOLO模型配置 --------------------
# YOLO_MODEL_PATH = "/home/wyq/ros2_ws/weights/best.pt"#best2.pt
# YOLO_CONFIDENCE_THRESHOLD = 0.6
# 新增RKNN全局变量
rknn_model = None
RKNN_MODEL_PATH = "/home/neardi/yolov8_seg_demo/yolov8n_seg_fp32.rknn"
RKNN_INPUT_SIZE = (640, 640)
RKNN_CONF_THRESH = 0.5
RKNN_IOU_THRESH = 0.45
CLASSES = ['pig']
TARGET_CLASS_ID = 0 # 'person' 或修改为其他类别ID
# -------------------- 相机话题配置 --------------------
COLOR_TOPIC = "/camera/color/image_raw"
DEPTH_TOPIC = "/camera/depth/image_raw"
CAMERA_INFO_TOPIC = "/camera/color/camera_info"
END_EFFECTOR_POSE_TOPIC = "/end_effector_pose"
# -------------------- 发布话题配置 --------------------
POINTCLOUD_TOPIC = "/detection/pointcloud"
RESULT_IMAGE_TOPIC = "/detection/result_image"
FINAL_POSE_TOPIC = "/detection/final_pose"
ACTION_NAME = "/vision/detection_pose_cloud"
# -------------------- 处理间隔配置 --------------------
PROCESS_INTERVAL = 0.3
# -------------------- 深度图配置 --------------------
DEPTH_MIN = 0.45
DEPTH_MAX = 3.0
DEPTH_SCALE = 1000.0
# -------------------- 点云配置 --------------------
VOXEL_SIZE = 0.005
MIN_MASK_PIXELS = 100
# 常量定义（放全局，不要函数内重复创建）
MIN_MASK_PIXELS = 100
DEPTH_MAX = 3000
SAMPLE_MAX_POINTS = 10000
# -------------------- 平面分割配置 (RANSAC) --------------------
RANSAC_DISTANCE_THRESHOLD = 0.002
RANSAC_N = 3
RANSAC_ITERATIONS = 500
# -------------------- 盒子尺寸配置 (目标物体) --------------------
BOX_LENGTH = 0.15 # 长度(米)
BOX_WIDTH = 0.056 # 宽度(米)
BOX_HEIGHT = 0.10 # 高度(米)
# -------------------- 尺寸匹配容差 --------------------
SIZE_MATCH_TOLERANCE = 0.03 # 尺寸匹配容差(米)
# -------------------- 可视化配置 --------------------
ENABLE_DISPLAY_WINDOW = False
ENABLE_3D_VISUALIZATION = False
WINDOW_WIDTH = 800
WINDOW_HEIGHT = 600
# -------------------- 坐标系可视化配置 --------------------
AXIS_LENGTH = 0.15
AXIS_LABEL_SCALE = 1.5
SHOW_BASE_FRAME = True
SHOW_WORLD_ORIGIN = True
BASE_FRAME_OFFSET = np.array([0.3, 0, 0])
# -------------------- 点云保存配置 --------------------
ENABLE_SAVE_PLY = False
SAVE_PLY_DIR = "."
# -------------------- 点云发布配置 --------------------
PUBLISH_POINTCLOUD = True
# -------------------- 形态学操作配置 --------------------
MORPH_KERNEL_SIZE = 5
MORPH_DILATE_ITERATIONS = 2
# -------------------- 降采样配置 --------------------
SAMPLE_MAX_POINTS = 20000
# ============================================================================
# 🔑 手眼标定外参配置
# ============================================================================
HAND_EYE_ROTATION = np.array([
[0.00271188, -0.01911421, 0.99981363, ],
[-0.99982551, -0.01853091, 0.00235764],
[0.0184824, -0.99964556, -0.01916113]
])
HAND_EYE_TRANSLATION = np.array([0.00730634, 0.00320815, 0.00048779]).reshape(3, 1)
HAND_EYE_MATRIX = np.eye(4)
HAND_EYE_MATRIX[:3, :3] = HAND_EYE_ROTATION
HAND_EYE_MATRIX[:3, 3:4] = HAND_EYE_TRANSLATION
ENABLE_HAND_EYE_TRANSFORM = True
M_TO_MM = 1000.0
QOS_PROFILE = QoSProfile(depth=10)
# ============================================================================
# 🔑 姿态偏移参数
# ============================================================================
PI = 3.1415926 # pai，角度修正
x_pianyi = -30.0 # 基础坐标系下x方向偏移
y_pianyi = 20.0 # 基础坐标系下y方向偏移
z_pianyi = 0.0 # 基础坐标系下z方向偏移
yaw_buchang = 5 # 基础坐标系下绕z旋转（步长角度为yaw_buchang）
pitch_buchang = 0 # 基础坐标系下绕y旋转（步长角度为pitch_buchang）
roll_buchang = 0 # 基础坐标系下绕x旋转（步长角度为roll_buchang）
# ============================================================================
# rknn模型函数
# ============================================================================
def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))
def xywh2xyxy(x):
y = np.zeros_like(x)
y[:,0] = x[:,0] - x[:,2] / 2
y[:,1] = x[:,1] - x[:,3] / 2
y[:,2] = x[:,0] + x[:,2] / 2
y[:,3] = x[:,1] + x[:,3] / 2
return y
def nms_cv2(boxes, scores, conf_thr, iou_thr):
if len(boxes) == 0:
return np.array([])
boxes_list = boxes.tolist()
scores_list = scores.tolist()
idx = cv2.dnn.NMSBoxes(boxes_list, scores_list, conf_thr, iou_thr)
return idx.flatten()
# RKNN推理封装：输入BGR原图，返回 (xyxy\_bbox, mask, valid)
def rknn_infer(rknn_model, rgb_origin, target_class_id=None):
"""
RKNN推理封装：输入BGR原图，返回 (bbox, mask, success)
target_class_id: 指定检测的目标类别ID，如果为None则返回置信度最高的
"""
h_ori, w_ori = rgb_origin.shape[:2]
W, H = RKNN_INPUT_SIZE
img_resize = cv2.resize(rgb_origin, RKNN_INPUT_SIZE)
img_rgb = cv2.cvtColor(img_resize, cv2.COLOR_BGR2RGB)
input_data = np.expand_dims(img_rgb, axis=0).astype(np.uint8)
outs = rknn_model.inference([input_data])
det_out = outs[0][0].T # (8400, 116)
proto = outs[1][0] # (32, 160, 160)
xywh_all = det_out[:, :4]
cls_logits = det_out[:, 4:84]
mask_coeff_all = det_out[:, 84:116]
conf_all = np.max(sigmoid(cls_logits), axis=1)
cls_id_all = np.argmax(cls_logits, axis=1)
# 根据目标类别过滤
if target_class_id is not None:
class_mask = (cls_id_all == target_class_id)
conf_filtered = conf_all * class_mask.astype(np.float32)
else:
conf_filtered = conf_all
valid_mask = conf_filtered > RKNN_CONF_THRESH
xywh_valid = xywh_all[valid_mask]
conf_valid = conf_filtered[valid_mask]
clsid_valid = cls_id_all[valid_mask]
mask_coeff_valid = mask_coeff_all[valid_mask]
if len(xywh_valid) == 0:
return None, None, False
xyxy_valid = xywh2xyxy(xywh_valid)
valid_box_mask = ~((xyxy_valid[:,0] >= W) | (xyxy_valid[:,1] >= H) |
(xyxy_valid[:,2] <= 0) | (xyxy_valid[:,3] <= 0))
xyxy_valid = xyxy_valid[valid_box_mask]
conf_valid = conf_valid[valid_box_mask]
clsid_valid = clsid_valid[valid_box_mask]
mask_coeff_valid = mask_coeff_valid[valid_box_mask]
if len(xyxy_valid) == 0:
return None, None, False
nms_idx = nms_cv2(xyxy_valid, conf_valid, RKNN_CONF_THRESH, RKNN_IOU_THRESH)
if len(nms_idx) == 0:
return None, None, False
final_xyxy = xyxy_valid[nms_idx]
final_conf = conf_valid[nms_idx]
final_clsid = clsid_valid[nms_idx]
final_mask_coeff = mask_coeff_valid[nms_idx]
# 取置信度最高的目标
best_idx = np.argmax(final_conf)
x1, y1, x2, y2 = final_xyxy[best_idx].astype(int)
coeff = final_mask_coeff[best_idx]
cid = final_clsid[best_idx]
score = final_conf[best_idx]
# 生成掩码
mask_pred = np.matmul(coeff, proto.reshape(32, -1)).reshape(160, 160)
mask_pred = sigmoid(mask_pred)
mask_bin = (mask_pred > 0.5).astype(np.uint8)
mask_resized = cv2.resize(mask_bin, RKNN_INPUT_SIZE, interpolation=cv2.INTER_LINEAR)
# 框内掩码清零杂边
box_mask = np.zeros_like(mask_resized)
x1 = np.clip(x1, 0, W-1)
y1 = np.clip(y1, 0, H-1)
x2 = np.clip(x2, 0, W-1)
y2 = np.clip(y2, 0, H-1)
box_mask[y1:y2, x1:x2] = 1
mask_final = np.logical_and(mask_resized, box_mask).astype(np.uint8)
# 映射回原图尺寸
mask_origin = cv2.resize(mask_final, (w_ori, h_ori), interpolation=cv2.INTER_LINEAR)
bbox_origin = np.array([
x1 / W * w_ori,
y1 / H * h_ori,
x2 / W * w_ori,
y2 / H * h_ori
]).astype(int)
return bbox_origin, mask_origin, True
# ============================================================================
# 辅助函数
# ============================================================================
def euler_zyx_to_quaternion(
roll: float, pitch: float, yaw: float, degrees: bool = False
) -> np.ndarray:
"""
将欧拉角 (ZYX 顺序) 转换为四元数。
Parameters
----------
roll : float
滚转角 (绕 X 轴), 弧度或角度
pitch : float
俯仰角 (绕 Y 轴), 弧度或角度
yaw : float
偏航角 (绕 Z 轴), 弧度或角度
degrees : bool, optional
如果为 True, 则 roll/pitch/yaw 的单位为度; 默认为 False (弧度)
Returns
-------
np.ndarray
四元数 [w, x, y, z]
Examples
--------
>>> # 绕 Z 轴旋转 90° (纯偏航)
>>> q = euler_zyx_to_quaternion(0, 0, 90, degrees=True)
>>> print(q) # [0.7071, 0, 0, 0.7071]
>>> # 绕 X 轴旋转 180° (纯滚转)
>>> q = euler_zyx_to_quaternion(180, 0, 0, degrees=True)
>>> print(q) # [0, 1, 0, 0]
"""
if degrees:
roll = math.radians(roll)
pitch = math.radians(pitch)
yaw = math.radians(yaw)
if yaw > -PI/2:
yaw = PI/2 - yaw
else:
yaw = PI/2 - yaw - PI
# 半角
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
# ZYX 顺序: q = q_z(yaw) * q_y(pitch) * q_x(roll)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([x, y, z, w])
def quaternion_to_euler_zyx(
q: np.ndarray, degrees: bool = False
) -> Tuple[float, float, float]:
"""
将四元数转换回欧拉角 (ZYX 顺序)。
Parameters
----------
q : np.ndarray
四元数 [w, x, y, z]
degrees : bool, optional
如果为 True, 返回角度制; 默认为 False (弧度)
Returns
-------
Tuple[float, float, float]
(roll, pitch, yaw)
Notes
-----
当 pitch 接近 ±90° 时存在万向锁, roll 和 yaw 可能不唯一。
"""
w, x, y, z = q
# 计算 pitch
sin_pitch = 2.0 * (w * y - z * x)
sin_pitch = np.clip(sin_pitch, -1.0, 1.0)
pitch = math.asin(sin_pitch)
# 计算 roll
sin_roll = 2.0 * (w * x + y * z)
cos_roll = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sin_roll, cos_roll)
# 计算 yaw
sin_yaw = 2.0 * (w * z + x * y)
cos_yaw = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(sin_yaw, cos_yaw)
if degrees:
roll = math.degrees(roll)
pitch = math.degrees(pitch)
yaw = math.degrees(yaw)
return roll, pitch, yaw
def normalize_angle_deg(angle_deg):
angle_deg = abs(angle_deg) % 180
if angle_deg > 90:
angle_deg = 180 - angle_deg
if angle_deg > 45:
angle_deg = 90 - angle_deg
return angle_deg
def normalize_angle_rad(angle_rad):
angle_rad = angle_rad % np.pi
if angle_rad > np.pi / 2:
angle_rad = np.pi - angle_rad
return angle_rad
def segment_display_and_publish(rgb, mask, bbox, bridge, publisher):
overlay = None
if not ENABLE_DISPLAY_WINDOW:
return None
try:
if rgb is None or len(rgb.shape) != 3:
return None
if rgb.dtype != np.uint8:
rgb = np.clip(rgb, 0, 255).astype(np.uint8)
overlay = rgb.copy()
if mask is not None and np.sum(mask) > 0:
if mask.dtype != np.uint8:
mask = mask.astype(np.uint8)
if mask.max() <= 1:
mask = mask * 255
mask_colored = np.zeros_like(rgb)
mask_colored[:, :, 1] = mask
overlay = cv2.addWeighted(rgb, 0.7, mask_colored, 0.3, 0)
if bbox is not None:
x1, y1, x2, y2 = bbox
cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.putText(overlay, "Target", (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
h, w = overlay.shape[:2]
cv2.putText(overlay, f"YOLO Detection | {w}x{h}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.imwrite("./result.png", overlay)
if ENABLE_DISPLAY_WINDOW:
cv2.namedWindow("Detection Result", cv2.WINDOW_NORMAL)
cv2.resizeWindow("Detection Result", WINDOW_WIDTH, WINDOW_HEIGHT)
cv2.imshow("Detection Result", overlay)
cv2.waitKey(1)
if publisher is not None and publisher.get_subscription_count() > 0:
try:
result_msg = bridge.cv2_to_imgmsg(overlay, "bgr8")
result_msg.header.stamp = bridge.get_clock().now().to_msg() if hasattr(bridge, 'get_clock') else None
result_msg.header.frame_id = "camera_link"
publisher.publish(result_msg)
print(f"📤 已发布检测结果图像")
except Exception as e:
print(f"发布检测结果图像失败: {e}")
except Exception as e:
print(f"segment_display错误: {e}")
return overlay
def calculate_plane_normal(plane_model):
[a, b, c, d] = plane_model
normal = np.array([a, b, c])
norm = np.linalg.norm(normal)
unit_normal = normal / norm
return normal, unit_normal
def ranscan(pcd):
if isinstance(pcd, np.ndarray):
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(pcd)
print(f"📊 已将NumPy数组 ({pcd.shape}) 转换为Open3D点云")
elif isinstance(pcd, o3d.geometry.PointCloud):
o3d_pcd = pcd
else:
raise TypeError(f"不支持的类型: {type(pcd)}")
if len(o3d_pcd.points) == 0:
print("❌ 点云为空")
return None, None, None
plane_model, inliers = o3d_pcd.segment_plane(
RANSAC_DISTANCE_THRESHOLD, RANSAC_N, RANSAC_ITERATIONS
)
[a, b, c, d] = plane_model
print(f"📐 Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
print(f"✅ 找到 {len(inliers)} 个内点 (占比 {len(inliers) / len(o3d_pcd.points) * 100:.1f}%)")
inlier_cloud = o3d_pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 0, 1.0])
outlier_cloud = o3d_pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([1.0, 0, 0])
return plane_model, inlier_cloud, outlier_cloud
# ==========================
# 🔑 智能轴检测函数 - 核心新增功能
# ==========================
def detect_box_axes_intelligently(points, plane_model, box_length, box_width, box_height, tolerance=0.03):
"""
智能检测盒子的三个轴方向
功能:
1. 从平面点云提取凸包
2. 计算凸包边长
3. 与真实尺寸匹配，确定哪个面被检测到
4. 自动确定Z轴（法向量方向）
5. 确定X轴（最长边）和Y轴（最短边）
6. 保持右手坐标系
参数:
points: 平面内点云 (N, 3)
plane_model: 平面方程 [a, b, c, d]
box_length, box_width, box_height: 盒子真实尺寸
tolerance: 尺寸匹配容差
返回:
x_axis, y_axis, z_axis: 三个轴的方向向量
detected_face: 检测到的面 ('top', 'side', 'front')
"""
# 提取法向量作为Z轴候选
[a, b, c, d] = plane_model
z_axis_candidate = np.array([a, b, c])
z_axis_candidate = z_axis_candidate / (np.linalg.norm(z_axis_candidate) + 1e-8)
# 确保Z轴朝上（如果法向量有垂直分量）
if abs(z_axis_candidate[2]) > 0.1:
if z_axis_candidate[2] < 0:
z_axis_candidate = -z_axis_candidate
# 投影点云到平面
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis_candidate), z_axis_candidate)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis_candidate)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis_candidate) * z_axis_candidate
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis_candidate, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
# 投影到2D
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
# 计算凸包
if len(points_2d) < 4:
return None, None, None, None
hull = ConvexHull(points_2d)
hull_vertices = points_2d[hull.vertices]
# 提取凸包的边长
edge_lengths_2d = []
edge_dirs_2d = []
for i in range(len(hull_vertices)):
j = (i + 1) % len(hull_vertices)
edge_vec = hull_vertices[j] - hull_vertices[i]
edge_len = np.linalg.norm(edge_vec)
if edge_len > 0.001:
edge_lengths_2d.append(edge_len)
edge_dirs_2d.append(edge_vec / edge_len)
# 如果边太少，无法判断
if len(edge_lengths_2d) < 2:
return None, None, None, None
# 找到最长边和最短边
sorted_indices = np.argsort(edge_lengths_2d)[::-1]
longest_edge_len = edge_lengths_2d[sorted_indices[0]]
shortest_edge_len = edge_lengths_2d[sorted_indices[-1]]
longest_dir = edge_dirs_2d[sorted_indices[0]]
shortest_dir = edge_dirs_2d[sorted_indices[-1]]
# 真实尺寸列表
real_sizes = sorted([box_length, box_width, box_height])
real_size_labels = {
box_length: 'length',
box_width: 'width',
box_height: 'height'
}
# 去重（如果有相同尺寸）
unique_sizes = sorted(set(real_sizes))
# 尝试匹配检测到的两个边长与真实尺寸
# 情况1: 检测到顶面 (长x宽)
# 情况2: 检测到侧面 (长x高 或 宽x高)
# 构建可能的匹配组合
possible_faces = []
# 顶面: 长x宽
if abs(longest_edge_len - box_length) < tolerance and abs(shortest_edge_len - box_width) < tolerance:
possible_faces.append(('top', box_length, box_width, box_height))
if abs(longest_edge_len - box_width) < tolerance and abs(shortest_edge_len - box_length) < tolerance:
possible_faces.append(('top', box_width, box_length, box_height))
# 侧面1: 长x高
if abs(longest_edge_len - box_length) < tolerance and abs(shortest_edge_len - box_height) < tolerance:
possible_faces.append(('side_lh', box_length, box_height, box_width))
if abs(longest_edge_len - box_height) < tolerance and abs(shortest_edge_len - box_length) < tolerance:
possible_faces.append(('side_lh', box_height, box_length, box_width))
# 侧面2: 宽x高
if abs(longest_edge_len - box_width) < tolerance and abs(shortest_edge_len - box_height) < tolerance:
possible_faces.append(('side_wh', box_width, box_height, box_length))
if abs(longest_edge_len - box_height) < tolerance and abs(shortest_edge_len - box_width) < tolerance:
possible_faces.append(('side_wh', box_height, box_width, box_length))
# 如果没有匹配，尝试更宽松的匹配或使用默认
if not possible_faces:
print("⚠️ 尺寸匹配失败，使用默认顶面假设")
# 默认假设检测到的是顶面
if box_length >= box_width:
possible_faces.append(('top', box_length, box_width, box_height))
else:
possible_faces.append(('top', box_width, box_length, box_height))
# 选择第一个匹配的
detected_face, size1, size2, size_z = possible_faces[0]
print(f"🔍 检测到面: {detected_face}")
print(f" 平面内边长1: {longest_edge_len:.4f}m -> 匹配 {size1:.4f}m")
print(f" 平面内边长2: {shortest_edge_len:.4f}m -> 匹配 {size2:.4f}m")
print(f" 法向量方向尺寸: {size_z:.4f}m")
# 确定轴方向
# Z轴: 法向量方向
z_axis = z_axis_candidate.copy()
# X轴: 最长边方向 (在平面内)
x_axis_2d = longest_dir
x_axis = x_axis_2d[0] * x_temp + x_axis_2d[1] * y_temp
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
# Y轴: 最短边方向 (在平面内)，确保与X轴垂直
y_axis_2d = shortest_dir
y_axis = y_axis_2d[0] * x_temp + y_axis_2d[1] * y_temp
y_axis = y_axis - np.dot(y_axis, x_axis) * x_axis # 正交化
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
# 🔑 关键: 确保右手坐标系 X × Y = Z
# 如果 X × Y 与 Z 方向相反，翻转 Y
cross_xy = np.cross(x_axis, y_axis)
if np.dot(cross_xy, z_axis) < 0:
y_axis = -y_axis
print("🔄 翻转Y轴以保持右手坐标系")
# 重新正交化
y_axis = y_axis - np.dot(y_axis, z_axis) * z_axis
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
# 根据检测到的面，调整Z轴方向
# 如果是侧面，法向量指向侧面，需要调整
if detected_face.startswith('side'):
# 对于侧面，法向量指向水平方向
# 保持Z轴指向与基坐标系Z轴一致（朝上）
if z_axis[2] < 0:
z_axis = -z_axis
# 重新计算X和Y
x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
print(f"📐 最终轴方向:")
print(f" X轴: ({x_axis[0]:.4f}, {x_axis[1]:.4f}, {x_axis[2]:.4f})")
print(f" Y轴: ({y_axis[0]:.4f}, {y_axis[1]:.4f}, {y_axis[2]:.4f})")
print(f" Z轴: ({z_axis[0]:.4f}, {z_axis[1]:.4f}, {z_axis[2]:.4f})")
return x_axis, y_axis, z_axis, detected_face
# ==========================
# 创建坐标系可视化函数
# ==========================
def create_world_origin(length=0.2):
geometries = []
origin_sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.015)
origin_sphere.translate([0, 0, 0])
origin_sphere.paint_uniform_color([0.3, 0.3, 0.3])
geometries.append(origin_sphere)
# X轴 (红色)
x_arrow = o3d.geometry.TriangleMesh.create_arrow(
cylinder_radius=0.008, cone_radius=0.015,
cylinder_height=length * 0.7, cone_height=length * 0.3
)
x_arrow.rotate(o3d.geometry.get_rotation_matrix_from_xyz([0, -np.pi / 2, 0]), center=[0, 0, 0])
x_arrow.translate([0, 0, 0])
x_arrow.paint_uniform_color([1.0, 0.0, 0.0])
geometries.append(x_arrow)
# Y轴 (绿色)
y_arrow = o3d.geometry.TriangleMesh.create_arrow(
cylinder_radius=0.008, cone_radius=0.015,
cylinder_height=length * 0.7, cone_height=length * 0.3
)
y_arrow.rotate(o3d.geometry.get_rotation_matrix_from_xyz([np.pi / 2, 0, 0]), center=[0, 0, 0])
y_arrow.translate([0, 0, 0])
y_arrow.paint_uniform_color([0.0, 1.0, 0.0])
geometries.append(y_arrow)
# Z轴 (蓝色)
z_arrow = o3d.geometry.TriangleMesh.create_arrow(
cylinder_radius=0.008, cone_radius=0.015,
cylinder_height=length * 0.7, cone_height=length * 0.3
)
z_arrow.translate([0, 0, 0])
z_arrow.paint_uniform_color([0.0, 0.0, 1.0])
geometries.append(z_arrow)
label_positions = [
([length * 1.1, 0, 0], [1.0, 0.0, 0.0]),
([0, length * 1.1, 0], [0.0, 1.0, 0.0]),
([0, 0, length * 1.1], [0.0, 0.0, 1.0]),
]
for pos, color in label_positions:
sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.01)
sphere.translate(pos)
sphere.paint_uniform_color(color)
geometries.append(sphere)
return geometries
def create_clear_coordinate_frame(origin, x_axis, y_axis, z_axis, length=0.15, prefix=''):
geometries = []
origin_sphere = o3d.geometry.TriangleMesh.create_sphere(radius=0.01)
origin_sphere.translate(origin)
origin_sphere.paint_uniform_color([0.5, 0.5, 0.5])
geometries.append(origin_sphere)
axis_configs = [
(x_axis, [1.0, 0.0, 0.0], 'X'),
(y_axis, [0.0, 1.0, 0.0], 'Y'),
(z_axis, [0.0, 0.0, 1.0], 'Z'),
]
for axis, color, label in axis_configs:
end_point = origin + axis * length
cylinder = o3d.geometry.TriangleMesh.create_cylinder(
radius=0.004,
height=length * 0.75
)
z_default = np.array([0, 0, 1])
if np.linalg.norm(axis) > 1e-8 and not np.allclose(axis, z_default):
v = np.cross(z_default, axis)
s = np.linalg.norm(v)
c = np.dot(z_default, axis)
if s > 1e-8:
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_matrix = np.eye(3) + vx + vx @ vx * ((1 - c) / (s * s))
cylinder.rotate(rot_matrix, center=[0, 0, 0])
cylinder.translate(origin + axis * length * 0.125)
cylinder.paint_uniform_color(color)
geometries.append(cylinder)
cone = o3d.geometry.TriangleMesh.create_cone(
radius=0.01,
height=length * 0.25
)
if np.linalg.norm(axis) > 1e-8 and not np.allclose(axis, z_default):
v = np.cross(z_default, axis)
s = np.linalg.norm(v)
c = np.dot(z_default, axis)
if s > 1e-8:
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_matrix = np.eye(3) + vx + vx @ vx * ((1 - c) / (s * s))
cone.rotate(rot_matrix, center=[0, 0, 0])
cone.translate(end_point - axis * length * 0.25)
cone.paint_uniform_color(color)
geometries.append(cone)
label_pos = end_point + axis * (length * 0.15)
bg = o3d.geometry.TriangleMesh.create_sphere(radius=0.012)
bg.translate(label_pos)
bg.paint_uniform_color([0.2, 0.2, 0.2])
geometries.append(bg)
core = o3d.geometry.TriangleMesh.create_sphere(radius=0.008)
core.translate(label_pos)
core.paint_uniform_color(color)
geometries.append(core)
return geometries
# ==========================
# 6D位姿估计器 (修改版)
# ==========================
class Box6DPoseEstimator:
def __init__(self, box_length, box_width, box_height):
self.box_length = box_length
self.box_width = box_width
self.box_height = box_height
self.extrinsic_matrix = None
self.end_effector_pose = None
def set_extrinsic_matrix(self, extrinsic):
self.extrinsic_matrix = np.array(extrinsic)
def set_end_effector_pose(self, pose):
self.end_effector_pose = np.array(pose)
def estimate_6d_pose(self, plane_model, inlier_cloud, box_center_camera=None):
"""
估计6D位姿 - 使用智能轴检测
"""
points = np.asarray(inlier_cloud.points)
# ============================================================
# 🔑 使用智能轴检测
# ============================================================
x_axis, y_axis, z_axis, detected_face = detect_box_axes_intelligently(
points, plane_model,
self.box_length, self.box_width, self.box_height,
tolerance=SIZE_MATCH_TOLERANCE
)
if x_axis is None or y_axis is None or z_axis is None:
print("❌ 轴检测失败，使用默认方法")
# 回退到原始方法
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if abs(z_axis[2]) > 0.1 and z_axis[2] < 0:
z_axis = -z_axis
x_axis, y_axis = self._extract_edges_pca(points, z_axis)
if x_axis is None or y_axis is None:
return None, None, None
print(f"📐 检测到的面: {detected_face}")
print(f"📐 X轴: ({x_axis[0]:.4f}, {x_axis[1]:.4f}, {x_axis[2]:.4f})")
print(f"📐 Y轴: ({y_axis[0]:.4f}, {y_axis[1]:.4f}, {y_axis[2]:.4f})")
print(f"📐 Z轴: ({z_axis[0]:.4f}, {z_axis[1]:.4f}, {z_axis[2]:.4f})")
# ============================================================
# 计算盒子中心位置
# ============================================================
top_center = np.mean(points, axis=0)
if box_center_camera is None:
# 根据检测到的面，调整中心位置
if detected_face == 'top':
# 顶面：中心在顶面中心 + 一半高度沿法向量
box_center_camera = top_center + z_axis * (self.box_height / 2.0)
elif detected_face.startswith('side'):
# 侧面：中心在侧面中心 + 一半宽度/高度沿法向量
# 使用检测到的平面尺寸判断
box_center_camera = top_center + z_axis * (self.box_width / 2.0)
else:
box_center_camera = top_center + z_axis * (self.box_height / 2.0)
print(f"📍 顶面中心: ({top_center[0]:.4f}, {top_center[1]:.4f}, {top_center[2]:.4f})")
print(f"📍 盒子中心: ({box_center_camera[0]:.4f}, {box_center_camera[1]:.4f}, {box_center_camera[2]:.4f})")
# ============================================================
# 构建旋转矩阵 (相机坐标系)
# ============================================================
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
# ============================================================
# 构建相机坐标系下的位姿矩阵
# ============================================================
pose_camera = np.eye(4)
pose_camera[:3, :3] = rotation_matrix
pose_camera[:3, 3] = box_center_camera
# ============================================================
# 坐标转换
# ============================================================
if self.extrinsic_matrix is not None:
pose_gripper = self.extrinsic_matrix @ pose_camera
else:
pose_gripper = pose_camera
if self.end_effector_pose is not None:
pose_base = self.end_effector_pose @ pose_gripper
else:
pose_base = pose_gripper
# ============================================================
# 提取6D位姿参数
# ============================================================
position = pose_base[:3, 3]
roll, pitch, yaw = self._rotation_matrix_to_euler(pose_base[:3, :3])
pose_6d = [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
# ============================================================
# 打印结果
# ============================================================
roll_deg = np.degrees(roll)
pitch_deg = np.degrees(pitch)
yaw_deg = np.degrees(yaw)
print(f"🎯 6D位姿估计结果 (位置单位: m):")
print(f" 位置 (x, y, z): ({pose_6d[0]:.4f}, {pose_6d[1]:.4f}, {pose_6d[2]:.4f}) m")
print(f" 位置 (x, y, z): ({pose_6d[0] * 1000:.1f}, {pose_6d[1] * 1000:.1f}, {pose_6d[2] * 1000:.1f}) mm")
print(f" 姿态: ({roll_deg:.2f}°, {pitch_deg:.2f}°, {yaw_deg:.2f}°)")
# 返回轴信息用于可视化
axes_dict = {
'x_axis': x_axis,
'y_axis': y_axis,
'z_axis': z_axis,
'origin': box_center_camera,
'detected_face': detected_face
}
return pose_6d, pose_camera, axes_dict
def _extract_edges_pca(self, points, z_axis):
"""PCA后备方案"""
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
return x_axis, y_axis
def _rotation_matrix_to_euler(self, R):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
roll, pitch, yaw = r.as_euler('zyx', degrees=False)
return roll, pitch, yaw
except:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def generate_box_model(self, pose=None):
l, w, h = self.box_length, self.box_width, self.box_height
vertices = np.array([
[-l / 2, -w / 2, -h / 2],
[l / 2, -w / 2, -h / 2],
[l / 2, w / 2, -h / 2],
[-l / 2, w / 2, -h / 2],
[-l / 2, -w / 2, h / 2],
[l / 2, -w / 2, h / 2],
[l / 2, w / 2, h / 2],
[-l / 2, w / 2, h / 2]
])
points = []
faces = [
([0, 1, 2, 3], [0, 0, -1]),
([4, 5, 6, 7], [0, 0, 1]),
([0, 1, 5, 4], [0, -1, 0]),
([2, 3, 7, 6], [0, 1, 0]),
([0, 3, 7, 4], [-1, 0, 0]),
([1, 2, 6, 5], [1, 0, 0])
]
for face_indices, normal in faces:
for i in np.linspace(0, 1, 10):
for j in np.linspace(0, 1, 10):
idx = face_indices
p = (1 - i) * (1 - j) * vertices[idx[0]] + i * (1 - j) * vertices[idx[1]] + \
i * j * vertices[idx[2]] + (1 - i) * j * vertices[idx[3]]
points.append(p)
points = np.array(points)
box_cloud = o3d.geometry.PointCloud()
box_cloud.points = o3d.utility.Vector3dVector(points)
if pose is not None:
if isinstance(pose, list) and len(pose) == 6:
matrix = self.pose_to_matrix(pose)
box_cloud.transform(matrix)
elif isinstance(pose, np.ndarray) and pose.shape == (4, 4):
box_cloud.transform(pose)
return box_cloud
def pose_to_matrix(self, pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = self._euler_to_rotation_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def _euler_to_rotation_matrix(self, roll, pitch, yaw):
try:
from scipy.spatial.transform import Rotation
#r = Rotation.from_euler(roll, pitch, yaw)
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except:
R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return R_z @ R_y @ R_x
def create_point_cloud(points_3d, colors, frame_id, clock):
if len(points_3d) == 0:
return PointCloud2()
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = clock.now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_3d)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_3d)
data = []
for pt, col in zip(points_3d, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
return cloud_msg
# ==========================
# 主节点
# ==========================
class CameraDetectionNode(Node):
def __init__(self):
super().__init__('ros2_camera_detection')
self.bridge = CvBridge()
self.rgb_img = None
self.depth_img = None
self.K = None
self.last_process_time = 0
self.process_interval = PROCESS_INTERVAL
self.end_effector_pose = None
self.end_effector_pose_matrix = None
self.end_effector_received = False
self.latest_points_3d = None
self.latest_colors = None
self.latest_pose_matrix = None
self.latest_pose_6d = None
self.latest_pose_gripper = None
self.latest_pose_base = None
self.latest_bbox = None
self.latest_inlier_cloud = None
self.latest_outlier_cloud = None
self.latest_box_model = None
self.latest_detection_img = None
self.latest_axes = None
self.latest_axes_base = None
self.latest_detected_face = None
self.pipeline_lock = threading.Lock()
self.load_hand_eye_calibration()
#self.yolo = YOLO(YOLO_MODEL_PATH)
#self.yolo.to("cpu")
#self.get_logger().info("✅ YOLO模型加载成功")
# ✅ 初始化 RKNN 模型
self.load_rknn_model()
self.create_subscription(Image, COLOR_TOPIC, self.rgb_cb, 10)
self.create_subscription(Image, DEPTH_TOPIC, self.depth_cb, 10)
self.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, self.caminfo_cb, 10)
self.create_subscription(PoseStamped, END_EFFECTOR_POSE_TOPIC, self.end_effector_pose_cb, 10)
self.pointcloud_pub = self.create_publisher(PointCloud2, POINTCLOUD_TOPIC, 10)
self.result_image_pub = self.create_publisher(Image, RESULT_IMAGE_TOPIC, 10)
self.final_pose_pub = self.create_publisher(PoseStamped, FINAL_POSE_TOPIC, 10)
self.action_server = ActionServer(
node=self,
action_name=ACTION_NAME,
action_type=VisionDetection,
execute_callback=self.execute_callback,
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback,
)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 节点初始化完成 (带智能轴检测)")
self.get_logger().info("=" * 60)
def load_rknn_model(self):
"""加载RKNN模型"""
try:
self.rknn = RKNNLite()
ret = self.rknn.load_rknn(RKNN_MODEL_PATH)
if ret != 0:
self.get_logger().error(f"❌ RKNN模型加载失败: {RKNN_MODEL_PATH}")
self.rknn = None
return
ret = self.rknn.init_runtime()
if ret != 0:
self.get_logger().error(f"❌ RKNN运行时初始化失败")
self.rknn.release()
self.rknn = None
return
self.get_logger().info(f"✅ RKNN模型加载成功: {RKNN_MODEL_PATH}")
self.get_logger().info(f" INPUT_SIZE: {RKNN_INPUT_SIZE}")
self.get_logger().info(f" CONF_THRESH: {RKNN_CONF_THRESH}")
except Exception as e:
self.get_logger().error(f"❌ RKNN初始化异常: {e}")
self.rknn = None
def load_hand_eye_calibration(self):
self.R_cam2gripper = HAND_EYE_ROTATION.copy()
self.t_cam2gripper = HAND_EYE_TRANSLATION.copy()
self.T_cam2gripper = HAND_EYE_MATRIX.copy()
self.enable_transform = ENABLE_HAND_EYE_TRANSFORM
det = np.linalg.det(self.R_cam2gripper)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 手眼标定参数加载完成")
self.get_logger().info(f"📐 旋转矩阵行列式: {det:.6f}")
self.get_logger().info("=" * 60)
def end_effector_pose_cb(self, msg: PoseStamped):
try:
position = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
quat = np.array(
[msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])
self.get_logger().info("=" * 60)
self.get_logger().info("📥 接收到末端位姿 (T_base2end):")
self.get_logger().info(
f" 位置: ({position[0] * 1000:.1f}, {position[1] * 1000:.1f}, {position[2] * 1000:.1f}) mm")
self.get_logger().info(f" 四元数: ({quat[0]:.4f}, {quat[1]:.4f}, {quat[2]:.4f}, {quat[3]:.4f})")
rotation = Rotation.from_quat(quat)
euler = rotation.as_euler('zyx', degrees=True)
self.get_logger().info(f" 姿态 (ZYX): ({euler[0]:.2f}°, {euler[1]:.2f}°, {euler[2]:.2f}°)")
self.get_logger().info("=" * 60)
self.end_effector_pose = {'position': position, 'quaternion': quat, 'timestamp': msg.header.stamp}
self.end_effector_pose_matrix = np.eye(4)
self.end_effector_pose_matrix[:3, :3] = rotation.as_matrix()
self.end_effector_pose_matrix[:3, 3] = position
self.end_effector_received = True
except Exception as e:
self.get_logger().error(f"末端位姿处理错误: {e}")
def transform_camera_to_gripper(self, pose_camera):
if not self.enable_transform:
return pose_camera
if isinstance(pose_camera, (list, np.ndarray)) and len(pose_camera) == 6:
T_camera = self.pose_6d_to_matrix(pose_camera)
elif isinstance(pose_camera, np.ndarray) and pose_camera.shape == (4, 4):
T_camera = pose_camera
else:
raise ValueError("输入格式错误")
T_gripper = self.T_cam2gripper @ T_camera
position = T_gripper[:3, 3]
rotation = T_gripper[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [float(position[0]), float(position[1]), float(position[2]), float(roll), float(pitch), float(yaw)]
def transform_gripper_to_base(self, pose_gripper):
if self.end_effector_pose_matrix is None:
return None
if isinstance(pose_gripper, (list, np.ndarray)) and len(pose_gripper) == 6:
T_gripper = self.pose_6d_to_matrix(pose_gripper)
elif isinstance(pose_gripper, np.ndarray) and pose_gripper.shape == (4, 4):
T_gripper = pose_gripper
else:
raise ValueError("输入格式错误")
T_base = self.end_effector_pose_matrix @ T_gripper
position = T_base[:3, 3]
rotation = T_base[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
self.get_logger().info(f"DEBUG raw base xyz(m): {position[0]}, {position[1]}, {position[2]}")
return [float(position[0]), float(position[1]), float(position[2]), float(roll), float(pitch), float(yaw)]
def get_gripper_pose(self, pose_camera):
if self.enable_transform:
return self.transform_camera_to_gripper(pose_camera)
else:
if isinstance(pose_camera, (list, np.ndarray)) and len(pose_camera) == 6:
return [float(v) for v in pose_camera]
elif isinstance(pose_camera, np.ndarray) and pose_camera.shape == (4, 4):
position = pose_camera[:3, 3]
rotation = pose_camera[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [float(position[0]), float(position[1]), float(position[2]), float(roll), float(pitch),
float(yaw)]
else:
raise ValueError("输入格式错误")
def pose_6d_to_matrix(self, pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = self.euler_to_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def euler_to_matrix(self, roll, pitch, yaw):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except ImportError:
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def matrix_to_euler(self, R):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
return r.as_euler('zyx')
except ImportError:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def publish_final_pose(self, pose_base):
if pose_base is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "base_link"
pos_x_mm = float(pose_base[0]) * M_TO_MM
pos_y_mm = float(pose_base[1]) * M_TO_MM
pos_z_mm = float(pose_base[2]) * M_TO_MM
pose_msg.pose.position.x = pos_x_mm + x_pianyi
pose_msg.pose.position.y = pos_y_mm + y_pianyi
pose_msg.pose.position.z = pos_z_mm + z_pianyi
# r = Rotation.from_euler('zyx', [pose_base[4], pose_base[4], pose_base[5]])
# quat = r.as_quat()
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
self.get_logger().info(f"🔄 四元数: [{quat[0]:.4f}, {quat[1]:.4f}, {quat[2]:.4f}, {quat[3]:.4f}]")
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
self.final_pose_pub.publish(pose_msg)
self.get_logger().info(f"📤 发布最终位姿: ({pos_x_mm:.1f}, {pos_y_mm:.1f}, {pos_z_mm:.1f}) mm")
def goal_callback(self, goal_request):
self.get_logger().info(f"📥 收到目标请求: {goal_request.target_obj_name}")
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle):
self.get_logger().info("❌ 任务取消")
return CancelResponse.ACCEPT
def caminfo_cb(self, msg):
self.K = np.array(msg.k).reshape(3, 3)
def rgb_cb(self, msg):
self.rgb_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
def depth_cb(self, msg):
try:
if msg.encoding == "16UC1":
depth_data = np.ndarray(shape=(msg.height, msg.width), dtype=np.uint16, buffer=msg.data)
else:
self.get_logger().warning(f"未知深度编码: {msg.encoding}")
return
self.depth_img = depth_data
self.process()
except Exception as e:
self.get_logger().error(f"深度图像处理错误: {e}")
def process(self):
if self.rgb_img is None or self.depth_img is None or self.K is None:
return
if self.rknn is None:
self.get_logger().warning("⚠️ RKNN模型未加载，跳过处理")
return
current_time = time.time()
if current_time - self.last_process_time < self.process_interval:
return
self.last_process_time = current_time
with self.pipeline_lock:
start = time.time()
rgb = self.rgb_img.copy()
h, w = rgb.shape[:2]
depth = self.depth_img.astype(np.float32) / DEPTH_SCALE
depth = np.clip(depth, DEPTH_MIN, DEPTH_MAX)
depth[np.isnan(depth)] = DEPTH_MIN
depth[np.isinf(depth)] = DEPTH_MIN
end1 = time.time()
target_id = TARGET_CLASS_ID # 默认检测'person'，根据需求修改
bbox, mask, success = rknn_infer(self.rknn, rgb, target_class_id=target_id)
if not success:
self.get_logger().warning("⚠️ RKNN检测失败或无目标")
return
# res = self.yolo.predict(rgb, conf=YOLO_CONFIDENCE_THRESHOLD, verbose=False)
# bbox = None
# if len(res) < 0:
# return
# if res[0].boxes is not None and len(res[0].boxes) > 0:
# box = res[0].boxes[0].xyxy.cpu().numpy()[0]
# x1, y1, x2, y2 = map(int, box)
# bbox = (x1, y1, x2, y2)
# self.get_logger().info(f"🎯 检测到目标: ({x1},{y1})-({x2},{y2})")
# if res[0].masks is None or len(res[0].masks) <= 0:
# return
# m = res[0].masks.data[0].cpu().numpy()
# m = cv2.resize(m, (w, h))
# mask = (m > 0.5).astype(np.uint8)
end2 = time.time()
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))
mask = cv2.dilate(mask, kernel, iterations=MORPH_DILATE_ITERATIONS)
mask_pixels = np.sum(mask)
self.get_logger().info(f"📐 掩码像素数: {mask_pixels}")
end3 = time.time()
overlay = segment_display_and_publish(rgb, mask, bbox, self.bridge, self.result_image_pub)
end4 = time.time()
if mask_pixels < MIN_MASK_PIXELS:
return
points_3d, colors = self.generate_pointcloud(rgb, depth, mask)
end5 = time.time()
if len(points_3d) <= 0:
return
if ENABLE_SAVE_PLY:
self.save_ply(points_3d, colors, os.path.join(SAVE_PLY_DIR, "detection_camera.ply"))
if PUBLISH_POINTCLOUD:
self.publish_pointcloud(points_3d, colors)
end6 = time.time()
plane_model, inlier_cloud, outlier_cloud = ranscan(points_3d)
end7 = time.time()
if plane_model is None:
return
normal, unit_normal = calculate_plane_normal(plane_model)
print(f"📐 法向量: ({unit_normal[0]:.4f}, {unit_normal[1]:.4f}, {unit_normal[2]:.4f})")
end8 = time.time()
box_estimator = Box6DPoseEstimator(
box_length=BOX_LENGTH,
box_width=BOX_WIDTH,
box_height=BOX_HEIGHT
)
pose_camera_6d, pose_camera_matrix, axes_dict = box_estimator.estimate_6d_pose(plane_model, inlier_cloud)
if pose_camera_6d is None:
self.get_logger().error("❌ 位姿估计失败")
return
self.latest_axes = axes_dict
if axes_dict is not None:
self.latest_detected_face = axes_dict.get('detected_face', 'unknown')
self.get_logger().info(f"🔍 检测到的面: {self.latest_detected_face}")
pose_gripper_6d = None
try:
pose_gripper_6d = self.get_gripper_pose(pose_camera_6d)
self.get_logger().info("✅ 成功转换到末端坐标系")
except Exception as e:
self.get_logger().error(f"❌ 转换到末端坐标系失败: {e}")
pose_gripper_6d = pose_camera_6d
pose_base_6d = None
pose_base_matrix = None
axes_base_dict = None
try:
if self.end_effector_pose_matrix is not None:
pose_base_6d = self.transform_gripper_to_base(pose_gripper_6d)
pose_base_matrix = self.pose_6d_to_matrix(pose_base_6d)
if axes_dict is not None:
R_base = pose_base_matrix[:3, :3]
x_axis_base = R_base[:, 0]
y_axis_base = R_base[:, 1]
z_axis_base = R_base[:, 2]
origin_base = pose_base_matrix[:3, 3]
axes_base_dict = {
'x_axis': x_axis_base,
'y_axis': y_axis_base,
'z_axis': z_axis_base,
'origin': origin_base,
'detected_face': self.latest_detected_face
}
self.get_logger().info("✅ 成功转换到基坐标系")
else:
self.get_logger().warn("⚠️ 未收到末端位姿，无法转换到基坐标系")
pose_base_6d = pose_gripper_6d
pose_base_matrix = pose_camera_matrix
axes_base_dict = axes_dict
except Exception as e:
self.get_logger().error(f"❌ 转换到基坐标系失败: {e}")
pose_base_6d = pose_gripper_6d
pose_base_matrix = pose_camera_matrix
axes_base_dict = axes_dict
self.latest_axes_base = axes_base_dict
# 打印结果
self.get_logger().info("=" * 60)
self.get_logger().info("🎯 位姿转换结果 (位置单位: mm):")
roll_cam = np.degrees(pose_camera_6d[3])
pitch_cam = np.degrees(pose_camera_6d[4])
yaw_cam = np.degrees(pose_camera_6d[5])
self.get_logger().info(
f" 📍 相机坐标系: ({pose_camera_6d[0] * M_TO_MM:.1f}, {pose_camera_6d[1] * M_TO_MM:.1f}, {pose_camera_6d[2] * M_TO_MM:.1f}) mm "
f"姿态: ({roll_cam:.2f}°, {pitch_cam:.2f}°, {yaw_cam:.2f}°)"
)
if pose_gripper_6d is not None:
roll_gripper = np.degrees(pose_gripper_6d[3])
pitch_gripper = np.degrees(pose_gripper_6d[4])
yaw_gripper = np.degrees(pose_gripper_6d[5])
self.get_logger().info(
f" 📍 末端坐标系: ({pose_gripper_6d[0] * M_TO_MM:.1f}, {pose_gripper_6d[1] * M_TO_MM:.1f}, {pose_gripper_6d[2] * M_TO_MM:.1f}) mm "
f"姿态: ({roll_gripper:.2f}°, {pitch_gripper:.2f}°, {yaw_gripper:.2f}°)"
)
if pose_base_6d is not None:
roll_base = np.degrees(pose_base_6d[3])
pitch_base = np.degrees(pose_base_6d[4])
yaw_base = np.degrees(pose_base_6d[5])
self.get_logger().info(
f" 📍 基坐标系: ({pose_base_6d[0] * M_TO_MM:.1f}, {pose_base_6d[1] * M_TO_MM:.1f}, {pose_base_6d[2] * M_TO_MM:.1f}) mm "
f"姿态: ({roll_base:.2f}°, {pitch_base:.2f}°, {yaw_base:.2f}°)"
)
self.get_logger().info("=" * 60)
box_model_camera = box_estimator.generate_box_model(pose_camera_matrix)
box_model_camera.paint_uniform_color([0, 0.8, 0])
box_model_base = None
if pose_base_matrix is not None:
box_model_base = box_estimator.generate_box_model(pose_base_matrix)
box_model_base.paint_uniform_color([0.8, 0.8, 0])
self.latest_points_3d = points_3d
self.latest_colors = colors
self.latest_pose_matrix = pose_camera_matrix
self.latest_pose_6d = pose_camera_6d
self.latest_pose_gripper = pose_gripper_6d
self.latest_pose_base = pose_base_6d
self.latest_bbox = bbox
self.latest_inlier_cloud = inlier_cloud
self.latest_outlier_cloud = outlier_cloud
self.latest_box_model = box_model_camera
if pose_base_6d is not None:
self.publish_final_pose(pose_base_6d)
end = time.time()
# 3D可视化
if ENABLE_3D_VISUALIZATION:
geometries = [inlier_cloud, outlier_cloud]
if box_model_camera is not None:
geometries.append(box_model_camera)
if SHOW_BASE_FRAME and box_model_base is not None:
geometries.append(box_model_base)
if SHOW_WORLD_ORIGIN:
geometries.extend(create_world_origin(length=0.2))
if axes_dict is not None:
origin = axes_dict['origin']
x_axis = axes_dict['x_axis']
y_axis = axes_dict['y_axis']
z_axis = axes_dict['z_axis']
geometries.extend(create_clear_coordinate_frame(
origin, x_axis, y_axis, z_axis, length=AXIS_LENGTH
))
if SHOW_BASE_FRAME and axes_base_dict is not None:
origin_base = axes_base_dict['origin']
x_axis_base = axes_base_dict['x_axis']
y_axis_base = axes_base_dict['y_axis']
z_axis_base = axes_base_dict['z_axis']
geometries.extend(create_clear_coordinate_frame(
origin_base, x_axis_base, y_axis_base, z_axis_base,
length=AXIS_LENGTH * 1.2
))
print("\n" + "=" * 60)
print("🎨 颜色图例:")
print(" 🔴 红色 (X轴) = 长度方向")
print(" 🟢 绿色 (Y轴) = 宽度方向")
print(" 🔵 蓝色 (Z轴) = 高度方向 (法向量)")
print(" 🔵 蓝色点云 = 目标表面 (平面内点)")
print(" 🔴 红色点云 = 背景点 (平面外点)")
print(" 🟩 绿色盒子 = 相机坐标系下的位姿")
if SHOW_BASE_FRAME:
print(" 🟨 黄色盒子 = 基坐标系下的位姿")
if SHOW_WORLD_ORIGIN:
print(" 🌐 灰色原点 = 基坐标系原点 (0,0,0)")
if self.latest_detected_face:
print(f" 📐 检测到的面: {self.latest_detected_face}")
print("=" * 60 + "\n")
o3d.visualization.draw_geometries(
geometries,
window_name=f"6D位姿估计结果 (检测面: {self.latest_detected_face})",
width=WINDOW_WIDTH, height=WINDOW_HEIGHT,
left=50, top=50
)
print("1.深度图对齐: ", end1 - start)
print("2.YOLO检测: ", end2 - end1)
print("3.形态学操作: ", end3 - end2)
print("4.分割显示: ", end4 - end3)
print("5.生成点云: ", end5 - end4)
print("6.保存点云: ", end6 - end5)
print("7.平面分割: ", end7 - end6)
print("8.法向量计算: ", end8 - end7)
print("9.6D位姿估计: ", end - end8)
print("总运行时间: ", end - start)
def generate_pointcloud(self, rgb, depth, mask):
h, w = rgb.shape[: 2]
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
# 1. 掩码筛选有效像素坐标（替代np.where+循环）
valid_mask = (mask > 0) & (depth > 0) & (depth <= DEPTH_MAX)
valid_cnt = np.count_nonzero(valid_mask)
if valid_cnt < MIN_MASK_PIXELS:
self.get_logger().warning(f"⚠️ 掩码有效像素不足: {valid_cnt} < {MIN_MASK_PIXELS}")
return None, None
# 2. 批量生成全图uv网格，向量化计算XYZ（无任何循环）
u_coords, v_coords = np.meshgrid(np.arange(w), np.arange(h))
z_vals = depth[valid_mask]
u_valid = u_coords[valid_mask]
v_valid = v_coords[valid_mask]
# 反投影公式批量计算
x = (u_valid - cx) *z_vals / fx
y = (v_valid - cy) *z_vals
points_3d = np.column_stack([x, y, z_vals]).astype(np.float32)
# 3. 批量提取RGB颜色，替代循环单像素读取
colors = rgb[valid_mask][:, [2, 1, 0]].astype(np.uint8) # BGR->RGB
# 4. 随机降采样（逻辑不变，numpy向量化操作）
n_points = len(points_3d)
if n_points > SAMPLE_MAX_POINTS:
indices = np.random.choice(n_points, SAMPLE_MAX_POINTS, replace = False)
points_3d = points_3d[indices]
colors = colors[indices]
self.get_logger().info(f"✅ 降采样到 {len(points_3d)} 个点")
self.get_logger().info(f"✅ 原始点云生成: {len(points_3d)} 个点")
return points_3d, colors
# def generate_pointcloud(self, rgb, depth, mask):
# h, w = rgb.shape[:2]
# fx = self.K[0, 0]
# fy = self.K[1, 1]
# cx = self.K[0, 2]
# cy = self.K[1, 2]
# ys, xs = np.where(mask > 0)
# if len(ys) < MIN_MASK_PIXELS:
# self.get_logger().warning(f"⚠️ 掩码像素不足: {len(ys)} < {MIN_MASK_PIXELS}")
# return None, None
# points_3d = []
# colors = []
# for v, u in zip(ys, xs):
# z = depth[v, u]
# if z <= 0 or z > DEPTH_MAX:
# continue
# x = (u - cx) * z / fx
# y = (v - cy) * z / fy
# b, g, r = rgb[v, u]
# points_3d.append([x, y, z])
# colors.append([r, g, b])
# if len(points_3d) == 0:
# self.get_logger().warning("⚠️ 无有效点云数据")
# return None, None
# points_3d = np.array(points_3d, dtype=np.float32)
# colors = np.array(colors, dtype=np.uint8)
# if len(points_3d) > SAMPLE_MAX_POINTS:
# indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
# points_3d = points_3d[indices]
# colors = colors[indices]
# self.get_logger().info(f"✅ 降采样到 {len(points_3d)} 个点")
# self.get_logger().info(f"✅ 原始点云生成: {len(points_3d)} 个点")
# return points_3d, colors
def save_ply(self, points, colors, filename="detection.ply"):
if len(points) == 0:
self.get_logger().warning(f"⚠️ 点云为空，跳过保存: {filename}")
return
try:
os.makedirs(os.path.dirname(os.path.abspath(filename)), exist_ok=True)
points = points.astype(np.float32)
colors = colors.astype(np.uint8)
with open(filename, 'w') as f:
f.write("ply\nformat ascii 1.0\n")
f.write(f"element vertex {len(points)}\n")
f.write("property float x\nproperty float y\nproperty float z\n")
f.write("property uchar red\nproperty uchar green\nproperty uchar blue\n")
f.write("end_header\n")
for i in range(len(points)):
x, y, z = points[i]
r, g, b = colors[i] if colors is not None else (255, 255, 255)
f.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
if os.path.exists(filename):
file_size = os.path.getsize(filename)
self.get_logger().info(f"💾 点云已保存: {filename}")
self.get_logger().info(f" 📁 文件大小: {file_size / 1024:.2f} KB")
else:
self.get_logger().error(f"❌ 文件保存失败: {filename}")
except Exception as e:
self.get_logger().error(f"❌ 保存PLY失败: {filename} - {e}")
def publish_pointcloud(self, points_camera, colors):
if len(points_camera) == 0:
return
try:
points_base = self.transform_points_to_base(points_camera)
if points_base is None:
points_base = points_camera
frame_id = "camera_link"
else:
frame_id = "base_link"
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = self.get_clock().now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_base)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_base)
data = []
for pt, col in zip(points_base, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
self.pointcloud_pub.publish(cloud_msg)
self.get_logger().info(f"☁️ 点云已发布 ({frame_id}): {len(points_base)} 个点")
except Exception as e:
self.get_logger().error(f"发布点云失败: {e}")
def transform_points_to_base(self, points_camera):
if len(points_camera) == 0:
return points_camera
if self.end_effector_pose_matrix is None:
return None
try:
ones = np.ones((len(points_camera), 1))
points_homogeneous = np.hstack([points_camera, ones])
points_gripper = (self.T_cam2gripper @ points_homogeneous.T).T
points_base = (self.end_effector_pose_matrix @ points_gripper.T).T
return points_base[:, :3]
except Exception as e:
self.get_logger().error(f"点云转换失败: {e}")
return None
def execute_callback(self, goal_handle: ServerGoalHandle):
self.get_logger().info("=" * 60)
self.get_logger().info("🔴 EXECUTE_CALLBACK 被调用")
self.get_logger().info("=" * 60)
goal = goal_handle.request
result = VisionDetection.Result()
try:
feedback = VisionDetection.Feedback()
feedback.task_status = "Processing..."
feedback.progress_rate = 0.0
feedback.current_step_info = "Initializing..."
points_3d = self.latest_points_3d
self.get_logger().info(f"📊 缓存状态: points={points_3d is not None}")
if points_3d is None or len(points_3d) == 0:
self.get_logger().error("❌ 没有可用数据")
result.success = False
result.status_code = 1
result.error_message = "没有可用的检测结果"
goal_handle.abort()
return result
feedback.progress_rate = 0.3
feedback.current_step_info = "Processing 6D pose..."
goal_handle.publish_feedback(feedback)
result.success = True
result.status_code = 0
result.error_message = ""
result.execution_time = time.time() - self.last_process_time
result.header = Header()
result.header.stamp = self.get_clock().now().to_msg()
result.header.frame_id = "base_link"
if goal.need_6d_pose:
feedback.progress_rate = 0.6
feedback.current_step_info = "Creating pose message..."
goal_handle.publish_feedback(feedback)
result.target_6d_pose = PoseStamped()
result.target_6d_pose.header = Header()
result.target_6d_pose.header.stamp = self.get_clock().now().to_msg()
result.target_6d_pose.header.frame_id = "base_link"
if self.latest_pose_base is not None:
pose_base = self.latest_pose_base
result.target_6d_pose.pose.position.x = float(pose_base[0]) * M_TO_MM + x_pianyi
result.target_6d_pose.pose.position.y = float(pose_base[1]) * M_TO_MM + y_pianyi
result.target_6d_pose.pose.position.z = float(pose_base[2]) * M_TO_MM + z_pianyi
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
# r = Rotation.from_euler('zyx', [pose_base[3], pose_base[4], pose_base[5]])
# quat = r.as_quat()
result.target_6d_pose.pose.orientation.x = quat[0]
result.target_6d_pose.pose.orientation.y = quat[1]
result.target_6d_pose.pose.orientation.z = quat[2]
result.target_6d_pose.pose.orientation.w = quat[3]
result.pose_confidence = 0.95
self.get_logger().info(
f" ✅ 使用基坐标系位姿: ({pose_base[0] * M_TO_MM:.1f}, {pose_base[1] * M_TO_MM:.1f}, {pose_base[2] * M_TO_MM:.1f}) mm"
)
else:
centroid = np.mean(points_3d, axis=0)
result.target_6d_pose.pose.position.x = float(centroid[0]) * M_TO_MM + x_pianyi
result.target_6d_pose.pose.position.y = float(centroid[1]) * M_TO_MM + y_pianyi
result.target_6d_pose.pose.position.z = float(centroid[2]) * M_TO_MM + z_pianyi
result.target_6d_pose.pose.orientation.w = 1.0
result.pose_confidence = 0.5
self.get_logger().warn("⚠️ 使用降级方案")
if goal.need_env_point_cloud:
feedback.progress_rate = 0.8
feedback.current_step_info = "Creating point cloud..."
goal_handle.publish_feedback(feedback)
points_base = self.transform_points_to_base(points_3d)
if points_base is not None:
cloud_frame = "base_link"
cloud_points = points_base
else:
cloud_frame = "camera_link"
cloud_points = points_3d
result.env_point_cloud = create_point_cloud(
cloud_points,
self.latest_colors if self.latest_colors is not None else np.ones((len(cloud_points), 3),
dtype=np.uint8) * 255,
cloud_frame,
self.get_clock()
)
result.point_cloud_frame_id = cloud_frame
self.get_logger().info(f"☁️ 点云已创建，点数: {len(cloud_points)}, 坐标系: {cloud_frame}")
feedback.progress_rate = 1.0
feedback.task_status = "Completed"
feedback.current_step_info = "Done"
goal_handle.publish_feedback(feedback)
self.get_logger().info("✅ 返回结果")
goal_handle.succeed()
return result
except Exception as e:
self.get_logger().error(f"❌ execute_callback 异常: {e}")
import traceback
traceback.print_exc()
result.success = False
result.status_code = 3
result.error_message = str(e)
goal_handle.abort()
return result
def __del__(self):
cv2.destroyAllWindows()
def main(args=None):
rclpy.init(args=args)
node = CameraDetectionNode()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("接收到退出信号")
finally:
cv2.destroyAllWindows()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
将该代码复制到板子的项目中运行结果如下：
图片中的警告是没有接收到机器人的末端位姿，不影响结果验证。
7.gemini相机在rk3576上获取6d位姿
7.1 运行代码（单目标）
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2, PointField
from geometry_msgs.msg import PoseStamped
from com_interfaces.msg import CameraDriver
from rclpy.qos import QoSPresetProfiles
from cv_bridge import CvBridge
from std_msgs.msg import Header
import cv2
import numpy as np
import struct
import time
from rknnlite.api import RKNNLite
import open3d as o3d
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
from com_interfaces.action import VisionDetection
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle
import threading
import os
import math
from typing import Tuple
from datetime import datetime
# 屏蔽无关警告
os.environ["QT_LOGGING_RULES"] = "qt.fonts.warning=false"
os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
os.environ["CV_LOG_LEVEL"] = "FATAL"
# ============================================================================
# 可调参数
# ============================================================================
# 相机参数,将深度值向左平移23.75mm,将深度值向前平移6mm
camera_pingyi_x = 0.0 # 0.02375
camera_pingyi_z = 0.006
# RKNN模型配置
RKNN_MODEL_PATH = "/home/neardi/yolov8_seg_demo/yolov8n_seg_fp32_1.rknn"
RKNN_INPUT_SIZE = (640, 640)
RKNN_CONF_THRESH = 0.3
RKNN_IOU_THRESH = 0.45
TARGET_CLASS_ID = 0
# 发布话题
POINTCLOUD_TOPIC = "/detection/pointcloud"
RESULT_IMAGE_TOPIC = "/detection/result_image"
FINAL_POSE_TOPIC = "/detection/final_pose"
ACTION_NAME = "/vision/detection_pose_cloud"
# 处理间隔
PROCESS_INTERVAL = 0.3
# 深度图配置
DEPTH_MIN = 0.35
DEPTH_MAX = 3.0
DEPTH_SCALE = 1000.0
# 点云配置 - 优化：减少点数
MIN_MASK_PIXELS = 100
SAMPLE_MAX_POINTS = 5000 # 从20000减少到5000
# 平面分割配置 - 优化：减少迭代次数
RANSAC_DISTANCE_THRESHOLD = 0.0015
RANSAC_N = 3
RANSAC_ITERATIONS = 200 # 从500减少到200
# 盒子尺寸
BOX_LENGTH = 0.23
BOX_WIDTH = 0.13
BOX_HEIGHT = 0.145
SIZE_MATCH_TOLERANCE = 0.1
# 形态学操作配置
MORPH_KERNEL_SIZE = 3 # 从5减小到3
MORPH_DILATE_ITERATIONS = 1 # 从2减小到1
# ============================================================================
# 点云保存配置 - 优化：禁用点云保存
# ============================================================================
ENABLE_SAVE_PCD = False # 禁用点云保存以节省时间
ENABLE_SAVE_SEGMENTATION = True # 保留分割结果保存
SEG_SAVE_INTERVAL = 5 # 每5帧保存一次
PCD_SAVE_DIR = "./pcd_results"
SEG_SAVE_DIR = os.path.join(PCD_SAVE_DIR, "segmentation_results")
# ============================================================================
# 手眼标定外参
# ============================================================================
HAND_EYE_ROTATION = np.array([
[0.01571449, -0.30814851, 0.9512087],
[0.00216534, -0.95131259, -0.3082185],
[0.99987466, 0.00690311, -0.01428272]
])
HAND_EYE_TRANSLATION = np.array([0.10762206, 0.12872836, -0.01844746]).reshape(3, 1)
HAND_EYE_MATRIX = np.eye(4)
HAND_EYE_MATRIX[:3, :3] = HAND_EYE_ROTATION
HAND_EYE_MATRIX[:3, 3:4] = HAND_EYE_TRANSLATION
# ============================================================================
# 工具函数
# ============================================================================
def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))
def xywh2xyxy(x):
y = np.zeros_like(x)
y[:, 0] = x[:, 0] - x[:, 2] / 2
y[:, 1] = x[:, 1] - x[:, 3] / 2
y[:, 2] = x[:, 0] + x[:, 2] / 2
y[:, 3] = x[:, 1] + x[:, 3] / 2
return y
def nms_cv2(boxes, scores, conf_thr, iou_thr):
if len(boxes) == 0:
return np.array([])
idx = cv2.dnn.NMSBoxes(boxes.tolist(), scores.tolist(), conf_thr, iou_thr)
return idx.flatten() if idx is not None else np.array([])
def euler_zyx_to_quaternion(roll, pitch, yaw):
cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([w, x, y, z])
def matrix_to_euler(R):
try:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
if sy < 1e-6:
return np.arctan2(-R[1, 2], R[1, 1]), np.arctan2(-R[2, 0], sy), 0.0
return np.arctan2(R[2, 1], R[2, 2]), np.arctan2(-R[2, 0], sy), np.arctan2(R[1, 0], R[0, 0])
except:
return 0.0, 0.0, 0.0
def euler_to_matrix(roll, pitch, yaw):
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def pose_6d_to_matrix(pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = euler_to_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def create_point_cloud(points_3d, colors, frame_id, clock):
if len(points_3d) == 0:
return PointCloud2()
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = clock.now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_3d)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_3d)
data = []
for pt, col in zip(points_3d, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
return cloud_msg
# ============================================================================
# RKNN推理 - 优化：使用更快的推理设置
# ============================================================================
def rknn_infer(rknn_model, bgr_origin, target_class_id=None):
h_ori, w_ori = bgr_origin.shape[:2]
W, H = RKNN_INPUT_SIZE
img_resize = cv2.resize(bgr_origin, (W, H), interpolation=cv2.INTER_LINEAR)
img_rgb = cv2.cvtColor(img_resize, cv2.COLOR_BGR2RGB)
input_data = np.expand_dims(img_rgb, axis=0).astype(np.uint8)
outs = rknn_model.inference([input_data])
raw_det = outs[0][0]
proto = outs[1][0]
xywh_all = raw_det[:4, :].T
cls_logits = raw_det[4:5, :].T
mask_coeff_all = raw_det[5:37, :].T
conf_all = np.max(sigmoid(cls_logits), axis=1)
cls_id_all = np.argmax(cls_logits, axis=1)
if target_class_id is not None:
class_mask = (cls_id_all == target_class_id)
conf_filtered = conf_all * class_mask.astype(np.float32)
else:
conf_filtered = conf_all
valid_mask = conf_filtered > RKNN_CONF_THRESH
xywh_valid = xywh_all[valid_mask]
conf_valid = conf_filtered[valid_mask]
mask_coeff_valid = mask_coeff_all[valid_mask]
if len(xywh_valid) == 0:
return None, None, False
xyxy_valid = xywh2xyxy(xywh_valid)
valid_box_mask = ~((xyxy_valid[:, 0] >= W) | (xyxy_valid[:, 1] >= H) |
(xyxy_valid[:, 2] <= 0) | (xyxy_valid[:, 3] <= 0))
xyxy_valid = xyxy_valid[valid_box_mask]
conf_valid = conf_valid[valid_box_mask]
mask_coeff_valid = mask_coeff_valid[valid_box_mask]
if len(xyxy_valid) == 0:
return None, None, False
nms_idx = nms_cv2(xyxy_valid, conf_valid, RKNN_CONF_THRESH, RKNN_IOU_THRESH)
if len(nms_idx) == 0:
return None, None, False
best_idx = np.argmax(conf_valid[nms_idx])
final_idx = nms_idx[best_idx]
x1, y1, x2, y2 = xyxy_valid[final_idx].astype(int)
coeff = mask_coeff_valid[final_idx]
mask_pred = np.matmul(coeff[np.newaxis, :], proto.reshape(32, -1))[0]
mask_pred = mask_pred.reshape(160, 160)
mask_pred = sigmoid(mask_pred)
mask_bin = (mask_pred > 0.5).astype(np.uint8)
mask_resized = cv2.resize(mask_bin, (W, H), interpolation=cv2.INTER_LINEAR)
box_mask = np.zeros_like(mask_resized)
x1, y1, x2, y2 = np.clip([x1, y1, x2, y2], 0, W - 1)
box_mask[y1:y2, x1:x2] = 1
mask_final = np.logical_and(mask_resized, box_mask).astype(np.uint8)
mask_origin = cv2.resize(mask_final, (w_ori, h_ori), interpolation=cv2.INTER_LINEAR)
bbox_origin = np.array([x1 / W * w_ori, y1 / H * h_ori, x2 / W * w_ori, y2 / H * h_ori]).astype(int)
return bbox_origin, mask_origin, True
# ============================================================================
# 智能轴检测函数 - 优化：简化计算
# ============================================================================
def detect_box_axes_intelligently(points, plane_model, box_length, box_width, box_height, tolerance=0.03):
# 提取法向量作为Z轴
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if z_axis[2] < 0:
z_axis = -z_axis
# 使用PCA获取主方向
centroid = np.mean(points, axis=0)
points_centered = points - centroid
# 投影到平面
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
# PCA
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
# 绕X轴旋转180度：X朝右，Y朝前，Z朝下
R_current = np.column_stack([x_axis, y_axis, z_axis])
R_transform = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
R_new = R_transform @ R_current
x_new = R_new[:, 0]
y_new = R_new[:, 1]
z_new = R_new[:, 2]
# 确保轴方向
if x_new[0] > 0:
x_new = -x_new
if y_new[1] > 0:
y_new = -y_new
if z_new[2] > 0:
z_new = -z_new
# 正交化
rotation_matrix = np.column_stack([x_new, y_new, z_new])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
center = np.mean(points, axis=0)
axes_dict = {
'x_axis': rotation_matrix[:, 0],
'y_axis': rotation_matrix[:, 1],
'z_axis': rotation_matrix[:, 2],
'origin': center
}
return rotation_matrix[:, 0], rotation_matrix[:, 1], rotation_matrix[:, 2], axes_dict
# ============================================================================
# 主节点
# ============================================================================
class CameraDetectionNode(Node):
def __init__(self):
super().__init__('ros2_camera_detection')
self.bridge = CvBridge()
self.rgb_img = None
self.depth_img = None
self.K = None
self.depth_K = None
self.last_process_time = 0
self.latest_points_3d = None
self.latest_colors = None
self.latest_pose_6d = None
self.latest_inlier_mask = None
self.pipeline_lock = threading.Lock()
self.save_counter = 0
# 创建保存目录
if ENABLE_SAVE_SEGMENTATION:
os.makedirs(SEG_SAVE_DIR, exist_ok=True)
self.get_logger().info(f"📁 分割结果保存目录: {SEG_SAVE_DIR}")
# 加载RKNN
self.load_rknn_model()
# 订阅相机数据
self.create_subscription(
CameraDriver,
"/vision/camera_data",
self.camera_data_callback,
QoSPresetProfiles.SYSTEM_DEFAULT.value
)
# 发布话题
self.pointcloud_pub = self.create_publisher(PointCloud2, POINTCLOUD_TOPIC, 10)
self.result_image_pub = self.create_publisher(Image, RESULT_IMAGE_TOPIC, 10)
self.final_pose_pub = self.create_publisher(PoseStamped, FINAL_POSE_TOPIC, 10)
# Action Server
self.action_server = ActionServer(
node=self,
action_name=ACTION_NAME,
action_type=VisionDetection,
execute_callback=self.execute_callback,
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback,
)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 优化版位姿估计节点启动")
self.get_logger().info("=" * 60)
def load_rknn_model(self):
try:
self.rknn = RKNNLite()
ret = self.rknn.load_rknn(RKNN_MODEL_PATH)
if ret != 0:
self.get_logger().error(f"❌ RKNN模型加载失败")
self.rknn = None
return
ret = self.rknn.init_runtime()
if ret != 0:
self.get_logger().error(f"❌ RKNN运行时初始化失败")
self.rknn.release()
self.rknn = None
return
self.get_logger().info(f"✅ RKNN模型加载成功")
except Exception as e:
self.get_logger().error(f"❌ RKNN初始化异常: {e}")
self.rknn = None
def camera_data_callback(self, msg: CameraDriver):
try:
self.rgb_img = self.bridge.imgmsg_to_cv2(msg.rgb, 'bgr8')
depth_msg = msg.depth
if depth_msg.encoding == "16UC1":
self.depth_img = np.ndarray(
shape=(depth_msg.height, depth_msg.width),
dtype=np.uint16,
buffer=depth_msg.data
)
else:
return
self.K = np.array(msg.rgb_camera_info.k).reshape(3, 3)
if hasattr(msg, 'depth_camera_info') and msg.depth_camera_info:
self.depth_K = np.array(msg.depth_camera_info.k).reshape(3, 3)
else:
self.depth_K = self.K.copy()
self.process()
except Exception as e:
self.get_logger().error(f"回调错误: {e}")
def process(self):
if self.rgb_img is None or self.depth_img is None or self.K is None:
return
if self.rknn is None:
return
current_time = time.time()
if current_time - self.last_process_time < PROCESS_INTERVAL:
return
self.last_process_time = current_time
with self.pipeline_lock:
start = time.time()
rgb = self.rgb_img.copy()
depth = self.depth_img.astype(np.float32) / DEPTH_SCALE
depth[np.isnan(depth)] = 0
depth[np.isinf(depth)] = 0
depth[(depth < DEPTH_MIN) | (depth > DEPTH_MAX)] = 0
# 1. 目标检测
bbox, mask, success = rknn_infer(self.rknn, rgb, target_class_id=TARGET_CLASS_ID)
if not success:
self.get_logger().warning("⚠️ 未检测到目标")
return
end1 = time.time()
# 形态学操作
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))
mask = cv2.dilate(mask, kernel, iterations=MORPH_DILATE_ITERATIONS)
mask_pixels = np.sum(mask)
if mask_pixels < MIN_MASK_PIXELS:
self.get_logger().warning(f"⚠️ 掩码像素不足: {mask_pixels}")
return
end2 = time.time()
# 2. 生成点云 - 优化：直接使用numpy向量化
points_3d, colors = self.generate_pointcloud_fast(rgb, depth, mask)
if points_3d is None or len(points_3d) <= 0:
return
end3 = time.time()
# 3. RANSAC平面分割 - 优化：减少迭代次数
plane_model, inlier_cloud, _ = self.ransac_plane(points_3d)
if plane_model is None:
return
end4 = time.time()
# 4. 计算6D位姿 - 优化：直接使用内点点云
pose_camera_6d, axes_dict = self.estimate_6d_pose_fast(plane_model, inlier_cloud)
if pose_camera_6d is None:
self.get_logger().error("❌ 位姿估计失败")
return
end5 = time.time()
# 5. 转换到末端坐标系
pose_gripper_6d = self.transform_camera_to_gripper(pose_camera_6d)
end6 = time.time()
# 保存最新数据
self.latest_points_3d = points_3d
self.latest_colors = colors
self.latest_pose_6d = pose_gripper_6d
# 6. 发布结果 - 优化：减少发布频率
self.publish_result_image(rgb, mask, bbox)
self.publish_pointcloud(points_3d, colors)
self.publish_final_pose(pose_gripper_6d)
end7 = time.time()
# 7. 保存分割结果（每N帧保存一次）
if ENABLE_SAVE_SEGMENTATION:
self.save_counter += 1
if self.save_counter % SEG_SAVE_INTERVAL == 0:
self.save_segmentation_result(rgb, mask, bbox, pose_gripper_6d)
end8 = time.time()
# 打印优化后的时间
elapsed = time.time() - start
self.get_logger().info(f"⏱️ 检测: {(end1 - start) * 1000:.1f}ms | 形态学: {(end2 - end1) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 点云: {(end3 - end2) * 1000:.1f}ms | 平面分割: {(end4 - end3) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 位姿: {(end5 - end4) * 1000:.1f}ms | 手眼: {(end6 - end5) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 发布: {(end7 - end6) * 1000:.1f}ms | 保存: {(end8 - end7) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 总耗时: {elapsed * 1000:.1f}ms")
def generate_pointcloud_fast(self, rgb, depth, mask):
"""优化版点云生成 - 使用numpy向量化"""
h, w = rgb.shape[:2]
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
ys, xs = np.where(mask > 0)
if len(ys) < MIN_MASK_PIXELS:
return None, None
# 获取深度值
z_vals = depth[ys, xs]
valid_depth = z_vals > 0
if not np.any(valid_depth):
return None, None
ys = ys[valid_depth]
xs = xs[valid_depth]
z_vals = z_vals[valid_depth]
# 向量化计算
x_vals = (xs - cx) * z_vals / fx - camera_pingyi_x
y_vals = (ys - cy) * z_vals / fy
z_vals = z_vals + camera_pingyi_z
points_3d = np.column_stack([x_vals, y_vals, z_vals]).astype(np.float32)
colors = rgb[ys, xs].astype(np.uint8)
# 随机采样
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
return points_3d, colors
def ransac_plane(self, points):
if len(points) < 10:
return None, None, None
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
plane_model, inliers = pcd.segment_plane(
RANSAC_DISTANCE_THRESHOLD, RANSAC_N, RANSAC_ITERATIONS
)
if len(inliers) < 10:
return None, None, None
inlier_cloud = pcd.select_by_index(inliers)
return plane_model, inlier_cloud, None
# def estimate_6d_pose_fast(self, plane_model, inlier_cloud):
# """优化版6D位姿估计 - 简化计算"""
# points = np.asarray(inlier_cloud.points)
# if len(points) < 10:
# return None, None
#
# # 使用优化的轴检测
# x_axis, y_axis, z_axis, axes_dict = detect_box_axes_intelligently(
# points, plane_model, BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT
# )
#
# if x_axis is None:
# return None, None
#
# # 构建旋转矩阵
# rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
# center = np.mean(points, axis=0)
#
# # 提取6D位姿
# roll, pitch, yaw = matrix_to_euler(rotation_matrix)
# pose_6d = [center[0], center[1], center[2], roll, pitch, yaw]
#
# return pose_6d, axes_dict
def estimate_6d_pose_fast(self, plane_model, inlier_cloud):
points = np.asarray(inlier_cloud.points)
if len(points) < 10:
return None, None
# 智能轴检测
x_axis, y_axis, z_axis, detected_face = detect_box_axes_intelligently(
points, plane_model,
BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT,
tolerance=SIZE_MATCH_TOLERANCE
)
if x_axis is None or y_axis is None or z_axis is None:
# 回退到PCA方法
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if z_axis[2] < 0:
z_axis = -z_axis
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
detected_face = 'unknown'
# ============================================================
# 🔑 绕X轴旋转180度
# 变换：X -> X, Y -> -Y, Z -> -Z
# 结果：X朝右，Y朝前，Z朝下
# ============================================================
# 构建当前旋转矩阵
R_current = np.column_stack([x_axis, y_axis, z_axis])
# 绕X轴旋转180度的旋转矩阵
R_transform = np.array([
[1, 0, 0],
[0, -1, 0],
[0, 0, -1]
])
# 应用变换：R_new = R_transform @ R_current
R_new = R_transform @ R_current
# 提取新的轴
x_new = R_new[:, 0]
y_new = R_new[:, 1]
z_new = R_new[:, 2]
# 确保X轴朝右（x分量为正）
if x_new[0] > 0:
x_new = -x_new
# y_new = -y_new
# z_new = -z_new
# 确保Y轴朝前（y分量为正）
if y_new[1] > 0:
y_new = -y_new
# x_new = -x_new
# z_new = -z_new
# 确保Z轴朝下（z分量为负）
if z_new[2] > 0:
z_new = -z_new
# x_new = -x_new
# y_new = -y_new
# 重新正交化确保是有效的旋转矩阵
rotation_matrix = np.column_stack([x_new, y_new, z_new])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
# 位置：平面中心
center = np.mean(points, axis=0)
# 提取6D位姿
roll, pitch, yaw = matrix_to_euler(rotation_matrix)
pose_6d = [center[0], center[1], center[2], roll, pitch, yaw]
axes_dict = {
'x_axis': rotation_matrix[:, 0],
'y_axis': rotation_matrix[:, 1],
'z_axis': rotation_matrix[:, 2],
'origin': center,
'detected_face': detected_face
}
print(f"📐 调整后的轴方向 (相机坐标系):")
print(f" X轴 (朝右): ({rotation_matrix[0, 0]:.4f}, {rotation_matrix[1, 0]:.4f}, {rotation_matrix[2, 0]:.4f})")
print(f" Y轴 (朝前): ({rotation_matrix[0, 1]:.4f}, {rotation_matrix[1, 1]:.4f}, {rotation_matrix[2, 1]:.4f})")
print(f" Z轴 (朝下): ({rotation_matrix[0, 2]:.4f}, {rotation_matrix[1, 2]:.4f}, {rotation_matrix[2, 2]:.4f})")
print("坐标位置： ", center[0], center[1], center[2])
return pose_6d, axes_dict
def transform_camera_to_gripper(self, pose_camera_6d):
"""转换到手眼标定后的末端坐标系"""
T_camera = pose_6d_to_matrix(pose_camera_6d)
T_gripper = HAND_EYE_MATRIX @ T_camera
pos = T_gripper[:3, 3]
roll, pitch, yaw = matrix_to_euler(T_gripper[:3, :3])
return [pos[0], pos[1], pos[2], roll, pitch, yaw]
def save_segmentation_result(self, rgb, mask, bbox, pose_gripper_6d):
"""保存分割结果（固定文件名覆盖）"""
try:
filename = os.path.join(SEG_SAVE_DIR, "seg_result.jpg")
annotated = rgb.copy()
if mask is not None and np.sum(mask) > 0:
mask_vis = mask.astype(np.uint8) * 255
mask_colored = np.zeros_like(rgb)
mask_colored[:, :, 1] = mask_vis
annotated = cv2.addWeighted(rgb, 0.5, mask_colored, 0.5, 0)
contours, _ = cv2.findContours(mask_vis, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(annotated, contours, -1, (0, 255, 0), 2)
if bbox is not None and len(bbox) == 4:
x1, y1, x2, y2 = bbox
cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.putText(annotated, "Target", (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
if pose_gripper_6d is not None:
pos_x, pos_y, pos_z = pose_gripper_6d[0:3]
roll, pitch, yaw = np.degrees(pose_gripper_6d[3:6])
info_text = [
f"Pose (mm): ({pos_x * 1000:.1f}, {pos_y * 1000:.1f}, {pos_z * 1000:.1f})",
f"Angle (deg): ({roll:.1f}, {pitch:.1f}, {yaw:.1f})"
]
for i, text in enumerate(info_text):
cv2.putText(annotated, text, (10, 30 + i * 25),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
cv2.imwrite(filename, annotated)
except Exception as e:
self.get_logger().error(f"❌ 保存分割结果失败: {e}")
def publish_result_image(self, rgb, mask, bbox):
try:
annotated = rgb.copy()
if mask is not None and np.sum(mask) > 0:
mask_vis = mask.astype(np.uint8) * 255
mask_colored = np.zeros_like(rgb)
mask_colored[:, :, 1] = mask_vis
annotated = cv2.addWeighted(rgb, 0.7, mask_colored, 0.3, 0)
if bbox is not None:
x1, y1, x2, y2 = bbox
cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.putText(annotated, "Target", (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
msg = self.bridge.cv2_to_imgmsg(annotated, "bgr8")
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "camera_link"
self.result_image_pub.publish(msg)
except Exception as e:
self.get_logger().error(f"发布图像失败: {e}")
def publish_pointcloud(self, points_3d, colors):
if len(points_3d) == 0:
return
try:
cloud_msg = create_point_cloud(points_3d, colors, "camera_link", self.get_clock())
self.pointcloud_pub.publish(cloud_msg)
except Exception as e:
self.get_logger().error(f"发布点云失败: {e}")
def publish_final_pose(self, pose_gripper_6d):
if pose_gripper_6d is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "gripper_link"
pose_msg.pose.position.x = float(pose_gripper_6d[0])
pose_msg.pose.position.y = float(pose_gripper_6d[1])
pose_msg.pose.position.z = float(pose_gripper_6d[2])
quat = euler_zyx_to_quaternion(pose_gripper_6d[3], pose_gripper_6d[4], pose_gripper_6d[5])
pose_msg.pose.orientation.w = quat[0]
pose_msg.pose.orientation.x = quat[1]
pose_msg.pose.orientation.y = quat[2]
pose_msg.pose.orientation.z = quat[3]
self.final_pose_pub.publish(pose_msg)
def goal_callback(self, goal_request):
self.get_logger().info(f"📥 收到目标请求: {goal_request.target_obj_name}")
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle):
self.get_logger().info("❌ 任务取消")
return CancelResponse.ACCEPT
def execute_callback(self, goal_handle: ServerGoalHandle):
goal = goal_handle.request
result = VisionDetection.Result()
try:
points_3d = self.latest_points_3d
pose = self.latest_pose_6d
if points_3d is None or len(points_3d) == 0:
result.success = False
result.status_code = 1
result.error_message = "没有可用的检测结果"
goal_handle.abort()
return result
result.success = True
result.status_code = 0
result.error_message = ""
result.execution_time = time.time() - self.last_process_time
result.header = Header()
result.header.stamp = self.get_clock().now().to_msg()
result.header.frame_id = "gripper_link"
if goal.need_6d_pose and pose is not None:
result.target_6d_pose = PoseStamped()
result.target_6d_pose.header = Header()
result.target_6d_pose.header.stamp = self.get_clock().now().to_msg()
result.target_6d_pose.header.frame_id = "gripper_link"
result.target_6d_pose.pose.position.x = float(pose[0])
result.target_6d_pose.pose.position.y = float(pose[1])
result.target_6d_pose.pose.position.z = float(pose[2])
quat = euler_zyx_to_quaternion(pose[3], pose[4], pose[5])
result.target_6d_pose.pose.orientation.w = quat[0]
result.target_6d_pose.pose.orientation.x = quat[1]
result.target_6d_pose.pose.orientation.y = quat[2]
result.target_6d_pose.pose.orientation.z = quat[3]
result.pose_confidence = 0.95
if goal.need_env_point_cloud:
result.env_point_cloud = create_point_cloud(
points_3d,
self.latest_colors if self.latest_colors is not None else np.ones((len(points_3d), 3),
dtype=np.uint8) * 255,
"camera_link",
self.get_clock()
)
result.point_cloud_frame_id = "camera_link"
goal_handle.succeed()
return result
except Exception as e:
self.get_logger().error(f"❌ execute_callback 异常: {e}")
result.success = False
result.status_code = 3
result.error_message = str(e)
goal_handle.abort()
return result
def __del__(self):
cv2.destroyAllWindows()
def main(args=None):
rclpy.init(args=args)
node = CameraDetectionNode()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("接收到退出信号")
finally:
cv2.destroyAllWindows()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
7.2 调试代码（单目标）
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2, PointField
from geometry_msgs.msg import PoseStamped
from com_interfaces.msg import CameraDriver
from rclpy.qos import QoSPresetProfiles
from cv_bridge import CvBridge
from std_msgs.msg import Header
import cv2
import numpy as np
import struct
import time
from rknnlite.api import RKNNLite
import open3d as o3d
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
from com_interfaces.action import VisionDetection
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle
import threading
import os
import math
from typing import Tuple
from datetime import datetime
# 屏蔽无关警告
os.environ["QT_LOGGING_RULES"] = "qt.fonts.warning=false"
os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
os.environ["CV_LOG_LEVEL"] = "FATAL"
# ============================================================================
# 可调参数
# ============================================================================
# 相机参数,将深度值向左平移23.75mm,将深度值向前平移6mm
camera_pingyi_x = 0.02375
camera_pingyi_z = 0.006
# RKNN模型配置
RKNN_MODEL_PATH = "/home/neardi/yolov8_seg_demo/yolov8n_seg_fp32_1.rknn"
RKNN_INPUT_SIZE = (640, 640)
RKNN_CONF_THRESH = 0.3
RKNN_IOU_THRESH = 0.45
TARGET_CLASS_ID = 0
# 发布话题
POINTCLOUD_TOPIC = "/detection/pointcloud"
RESULT_IMAGE_TOPIC = "/detection/result_image"
FINAL_POSE_TOPIC = "/detection/final_pose"
ACTION_NAME = "/vision/detection_pose_cloud"
# 处理间隔
PROCESS_INTERVAL = 0.3
# 深度图配置
DEPTH_MIN = 0.35
DEPTH_MAX = 3.0
DEPTH_SCALE = 1000.0
# 点云配置
MIN_MASK_PIXELS = 100
# 平面分割配置
RANSAC_DISTANCE_THRESHOLD = 0.0015
RANSAC_N = 3
RANSAC_ITERATIONS = 500
# 盒子尺寸
BOX_LENGTH = 0.23
BOX_WIDTH = 0.13
BOX_HEIGHT = 0.145
SIZE_MATCH_TOLERANCE = 0.1
# 形态学操作配置
MORPH_KERNEL_SIZE = 5
MORPH_DILATE_ITERATIONS = 2
SAMPLE_MAX_POINTS = 20000
# ============================================================================
# 点云保存配置（新增）
# ============================================================================
ENABLE_SAVE_PCD = True # 启用点云保存
PCD_SAVE_DIR = "./pcd_results" # 保存目录
PCD_FILENAME = "combined_result.ply" # 固定文件名（相机坐标系）
PCD_GRIPPER_FILENAME = "combined_result_gripper.ply" # 末端坐标系文件名
INFO_FILENAME = "pose_info.txt" # 信息文件名
AXIS_LENGTH = 0.05 # 坐标轴长度（米）
POSE_POINT_RADIUS = 0.008 # 位姿点半径
AXIS_POINTS_PER_LINE = 30 # 每条轴线的点数
SPHERE_POINTS = 200 # 球体点数
# ============================================================================
# 手眼标定外参
# ============================================================================
HAND_EYE_ROTATION = np.array([
[0.01571449, -0.30814851, 0.9512087],
[0.00216534, -0.95131259, -0.3082185],
[0.99987466, 0.00690311, -0.01428272]
])
HAND_EYE_TRANSLATION = np.array([0.10762206, 0.12872836, -0.01844746]).reshape(3, 1)
HAND_EYE_MATRIX = np.eye(4)
HAND_EYE_MATRIX[:3, :3] = HAND_EYE_ROTATION
HAND_EYE_MATRIX[:3, 3:4] = HAND_EYE_TRANSLATION
M_TO_MM = 1.0
# ============================================================================
# 工具函数
# ============================================================================
def sigmoid(x):
return 1.0 / (1.0 + np.exp(-x))
def xywh2xyxy(x):
y = np.zeros_like(x)
y[:, 0] = x[:, 0] - x[:, 2] / 2
y[:, 1] = x[:, 1] - x[:, 3] / 2
y[:, 2] = x[:, 0] + x[:, 2] / 2
y[:, 3] = x[:, 1] + x[:, 3] / 2
return y
def nms_cv2(boxes, scores, conf_thr, iou_thr):
if len(boxes) == 0:
return np.array([])
idx = cv2.dnn.NMSBoxes(boxes.tolist(), scores.tolist(), conf_thr, iou_thr)
return idx.flatten() if idx is not None else np.array([])
def euler_zyx_to_quaternion(roll, pitch, yaw):
cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([w, x, y, z])
def quaternion_to_rotation_matrix(q):
w, x, y, z = q
return np.array([
[1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
[2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
[2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
])
def rotation_matrix_to_quaternion(R):
w = math.sqrt(1.0 + R[0, 0] + R[1, 1] + R[2, 2]) / 2.0
x = (R[2, 1] - R[1, 2]) / (4.0 * w)
y = (R[0, 2] - R[2, 0]) / (4.0 * w)
z = (R[1, 0] - R[0, 1]) / (4.0 * w)
return np.array([w, x, y, z])
def matrix_to_euler(R):
try:
q = rotation_matrix_to_quaternion(R)
w, x, y, z = q
sin_pitch = np.clip(2.0 * (w * y - z * x), -1.0, 1.0)
pitch = math.asin(sin_pitch)
roll = math.atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
return roll, pitch, yaw
except:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
if sy < 1e-6:
return np.arctan2(-R[1, 2], R[1, 1]), np.arctan2(-R[2, 0], sy), 0.0
return np.arctan2(R[2, 1], R[2, 2]), np.arctan2(-R[2, 0], sy), np.arctan2(R[1, 0], R[0, 0])
def euler_to_matrix(roll, pitch, yaw):
Rx = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return Rz @ Ry @ Rx
def pose_6d_to_matrix(pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = euler_to_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def create_point_cloud(points_3d, colors, frame_id, clock):
if len(points_3d) == 0:
return PointCloud2()
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = clock.now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_3d)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_3d)
data = []
for pt, col in zip(points_3d, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
return cloud_msg
# ============================================================================
# 点云保存功能（新增）- 使用固定文件名
# ============================================================================
def save_combined_pointcloud(
all_points, # 所有点云 (N, 3)
all_colors, # 所有点云颜色 (N, 3) RGB 0-255
inlier_mask, # 内点掩码 (N,) bool
pose_center, # 位姿中心 (3,)
x_axis, y_axis, z_axis, # 坐标轴方向
timestamp,
save_dir=PCD_SAVE_DIR
):
"""
保存合并的点云到单个PLY文件（使用固定文件名，覆盖之前保存的）
包含: 所有点云 + 位姿中心(绿色大点) + 坐标轴方向
内点显示为蓝色，外点显示为红色
"""
if not ENABLE_SAVE_PCD:
return
# 确保目录存在
os.makedirs(save_dir, exist_ok=True)
# 使用固定文件名
ply_filename = os.path.join(save_dir, PCD_FILENAME)
info_filename = os.path.join(save_dir, INFO_FILENAME)
print(f"\n{'=' * 60}")
print(f"💾 保存合并点云到: {ply_filename}")
print(f"{'=' * 60}")
# ============================================================
# 1. 准备所有点云数据（重新着色）
# ============================================================
combined_points = []
combined_colors = []
# 1.1 添加所有点云，内点为蓝色，外点为红色
if len(all_points) > 0:
for i, pt in enumerate(all_points):
combined_points.append(pt)
if inlier_mask[i]:
# 内点 - 蓝色 (BGR: 255, 0, 0)
combined_colors.append([0, 0, 255])
else:
# 外点 - 红色 (BGR: 0, 0, 255)
combined_colors.append([255, 0, 0])
# ============================================================
# 2. 生成位姿中心点（绿色大点 - 球体）
# ============================================================
sphere_points, sphere_colors = create_sphere_points(
pose_center,
radius=POSE_POINT_RADIUS,
num_points=SPHERE_POINTS,
color=[0, 255, 0] # 绿色
)
combined_points.extend(sphere_points)
combined_colors.extend(sphere_colors)
# ============================================================
# 3. 生成坐标轴（带箭头效果）
# ============================================================
# X轴 - 红色
axis_points, axis_colors = create_axis_points(
pose_center, x_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[255, 0, 0] # 红色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# Y轴 - 绿色
axis_points, axis_colors = create_axis_points(
pose_center, y_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[0, 255, 0] # 绿色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# Z轴 - 蓝色
axis_points, axis_colors = create_axis_points(
pose_center, z_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[0, 0, 255] # 蓝色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# ============================================================
# 4. 添加轴标签（小圆点）
# ============================================================
# X标签
label_pos = pose_center + x_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[255, 0, 0]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# Y标签
label_pos = pose_center + y_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[0, 255, 0]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# Z标签
label_pos = pose_center + z_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[0, 0, 255]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# ============================================================
# 5. 保存为PLY文件（覆盖模式）
# ============================================================
if len(combined_points) > 0:
combined_points = np.array(combined_points, dtype=np.float32)
combined_colors = np.array(combined_colors, dtype=np.uint8)
# 保存PLY
with open(ply_filename, 'w') as f:
f.write("ply\n")
f.write("format ascii 1.0\n")
f.write(f"element vertex {len(combined_points)}\n")
f.write("property float x\n")
f.write("property float y\n")
f.write("property float z\n")
f.write("property uchar red\n")
f.write("property uchar green\n")
f.write("property uchar blue\n")
f.write("end_header\n")
for i in range(len(combined_points)):
x, y, z = combined_points[i]
r, g, b = combined_colors[i]
f.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
file_size = os.path.getsize(ply_filename)
print(f"✅ 合并点云已保存: {ply_filename}")
print(f" 📁 文件大小: {file_size / 1024:.2f} KB")
print(f" 📊 总点数: {len(combined_points)}")
print(f" - 原始点云: {len(all_points)}")
print(f" - 内点(蓝色): {np.sum(inlier_mask)}")
print(f" - 外点(红色): {len(all_points) - np.sum(inlier_mask)}")
print(f" - 位姿中心: {SPHERE_POINTS}")
print(f" - 坐标轴: {AXIS_POINTS_PER_LINE * 3 * 2 + 90}") # 3条轴 + 3个标签
# ============================================================
# 6. 保存位姿信息到文本文件（覆盖模式）
# ============================================================
time_str = datetime.fromtimestamp(timestamp).strftime("%Y-%m-%d %H:%M:%S")
with open(info_filename, 'w') as f:
f.write("=" * 60 + "\n")
f.write("6D Pose Estimation Result\n")
f.write("=" * 60 + "\n")
f.write(f"Timestamp: {time_str}\n\n")
f.write("Position (m):\n")
f.write(f" x: {pose_center[0]:.6f}\n")
f.write(f" y: {pose_center[1]:.6f}\n")
f.write(f" z: {pose_center[2]:.6f}\n\n")
f.write("Position (mm):\n")
f.write(f" x: {pose_center[0] * 1000:.3f}\n")
f.write(f" y: {pose_center[1] * 1000:.3f}\n")
f.write(f" z: {pose_center[2] * 1000:.3f}\n\n")
f.write("Axis Directions:\n")
f.write(f" X-axis: ({x_axis[0]:.6f}, {x_axis[1]:.6f}, {x_axis[2]:.6f})\n")
f.write(f" Y-axis: ({y_axis[0]:.6f}, {y_axis[1]:.6f}, {y_axis[2]:.6f})\n")
f.write(f" Z-axis: ({z_axis[0]:.6f}, {z_axis[1]:.6f}, {z_axis[2]:.6f})\n\n")
# 计算欧拉角
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
roll, pitch, yaw = matrix_to_euler(rotation_matrix)
f.write("Euler Angles (rad):\n")
f.write(f" roll: {roll:.6f}\n")
f.write(f" pitch: {pitch:.6f}\n")
f.write(f" yaw: {yaw:.6f}\n\n")
f.write("Euler Angles (deg):\n")
f.write(f" roll: {np.degrees(roll):.3f}\n")
f.write(f" pitch: {np.degrees(pitch):.3f}\n")
f.write(f" yaw: {np.degrees(yaw):.3f}\n\n")
f.write("Point Cloud Statistics:\n")
f.write(f" Total points: {len(all_points)}\n")
f.write(f" Inlier points: {np.sum(inlier_mask)}\n")
f.write(f" Outlier points: {len(all_points) - np.sum(inlier_mask)}\n")
print(f"📄 位姿信息已保存到: {info_filename}")
print(f"{'=' * 60}\n")
def save_combined_pointcloud_gripper(
all_points, # 所有点云 (N, 3) - 已在末端坐标系
all_colors, # 所有点云颜色 (N, 3) RGB 0-255
inlier_mask, # 内点掩码 (N,) bool
pose_center, # 位姿中心 (3,) - 末端坐标系
x_axis, y_axis, z_axis, # 坐标轴方向 - 末端坐标系
timestamp,
save_dir=PCD_SAVE_DIR
):
"""
保存末端坐标系下的合并点云到单个PLY文件
包含: 所有点云 + 位姿中心(绿色大点) + 坐标轴方向
内点显示为蓝色，外点显示为红色
"""
if not ENABLE_SAVE_PCD:
return
# 确保目录存在
os.makedirs(save_dir, exist_ok=True)
# 使用固定文件名
ply_filename = os.path.join(save_dir, PCD_GRIPPER_FILENAME)
print(f"\n{'=' * 60}")
print(f"💾 保存末端坐标系点云到: {ply_filename}")
print(f"{'=' * 60}")
# ============================================================
# 1. 准备所有点云数据（重新着色）
# ============================================================
combined_points = []
combined_colors = []
# 1.1 添加所有点云，内点为蓝色，外点为红色
if len(all_points) > 0:
for i, pt in enumerate(all_points):
combined_points.append(pt)
if inlier_mask[i]:
# 内点 - 蓝色 (BGR: 255, 0, 0)
combined_colors.append([0, 0, 255])
else:
# 外点 - 红色 (BGR: 0, 0, 255)
combined_colors.append([255, 0, 0])
# ============================================================
# 2. 生成位姿中心点（绿色大点 - 球体）
# ============================================================
sphere_points, sphere_colors = create_sphere_points(
pose_center,
radius=POSE_POINT_RADIUS,
num_points=SPHERE_POINTS,
color=[0, 255, 0] # 绿色
)
combined_points.extend(sphere_points)
combined_colors.extend(sphere_colors)
# ============================================================
# 3. 生成坐标轴（带箭头效果）
# ============================================================
# X轴 - 红色
axis_points, axis_colors = create_axis_points(
pose_center, x_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[255, 0, 0] # 红色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# Y轴 - 绿色
axis_points, axis_colors = create_axis_points(
pose_center, y_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[0, 255, 0] # 绿色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# Z轴 - 蓝色
axis_points, axis_colors = create_axis_points(
pose_center, z_axis,
length=AXIS_LENGTH,
num_points=AXIS_POINTS_PER_LINE,
color=[0, 0, 255] # 蓝色
)
combined_points.extend(axis_points)
combined_colors.extend(axis_colors)
# ============================================================
# 4. 添加轴标签（小圆点）
# ============================================================
# X标签
label_pos = pose_center + x_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[255, 0, 0]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# Y标签
label_pos = pose_center + y_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[0, 255, 0]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# Z标签
label_pos = pose_center + z_axis * (AXIS_LENGTH * 1.15)
label_points, label_colors = create_sphere_points(
label_pos,
radius=0.005,
num_points=30,
color=[0, 0, 255]
)
combined_points.extend(label_points)
combined_colors.extend(label_colors)
# ============================================================
# 5. 保存为PLY文件（覆盖模式）
# ============================================================
if len(combined_points) > 0:
combined_points = np.array(combined_points, dtype=np.float32)
combined_colors = np.array(combined_colors, dtype=np.uint8)
# 保存PLY
with open(ply_filename, 'w') as f:
f.write("ply\n")
f.write("format ascii 1.0\n")
f.write(f"element vertex {len(combined_points)}\n")
f.write("property float x\n")
f.write("property float y\n")
f.write("property float z\n")
f.write("property uchar red\n")
f.write("property uchar green\n")
f.write("property uchar blue\n")
f.write("end_header\n")
for i in range(len(combined_points)):
x, y, z = combined_points[i]
r, g, b = combined_colors[i]
f.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
file_size = os.path.getsize(ply_filename)
print(f"✅ 末端坐标系点云已保存: {ply_filename}")
print(f" 📁 文件大小: {file_size / 1024:.2f} KB")
print(f" 📊 总点数: {len(combined_points)}")
print(f" - 原始点云: {len(all_points)}")
print(f" - 内点(蓝色): {np.sum(inlier_mask)}")
print(f" - 外点(红色): {len(all_points) - np.sum(inlier_mask)}")
print(f" - 位姿中心: {SPHERE_POINTS}")
print(f" - 坐标轴: {AXIS_POINTS_PER_LINE * 3 * 2 + 90}")
print(f"{'=' * 60}\n")
def create_sphere_points(center, radius, num_points, color):
"""
生成球体表面的点云
"""
points = []
colors = []
# 使用随机采样生成球面点
for i in range(num_points):
# 球坐标
theta = np.random.uniform(0, 2 * np.pi)
phi = np.random.uniform(0, np.pi)
x = center[0] + radius * np.sin(phi) * np.cos(theta)
y = center[1] + radius * np.sin(phi) * np.sin(theta)
z = center[2] + radius * np.cos(phi)
points.append([x, y, z])
colors.append(color)
return np.array(points), np.array(colors)
def create_axis_points(origin, direction, length, num_points, color):
"""
生成坐标轴的点云（轴线 + 箭头）
"""
points = []
colors = []
# 归一化方向向量
direction = direction / (np.linalg.norm(direction) + 1e-8)
# 轴线点
for i in range(num_points):
t = i / (num_points - 1) * length
point = origin + direction * t
points.append(point)
colors.append(color)
# 箭头（在末端加粗）
arrow_start = origin + direction * (length * 0.8)
for i in range(num_points // 2):
t = i / (num_points // 2 - 1) * (length * 0.2) if num_points // 2 > 1 else 0
# 箭头逐渐变粗
radius = 0.003 * (1 - t / (length * 0.2))
# 在箭头周围随机采样
for _ in range(5):
theta = np.random.uniform(0, 2 * np.pi)
# 垂直方向
if abs(direction[2]) < 0.9:
perp = np.cross(direction, [0, 0, 1])
else:
perp = np.cross(direction, [1, 0, 0])
perp = perp / (np.linalg.norm(perp) + 1e-8)
perp2 = np.cross(direction, perp)
offset = radius * (np.cos(theta) * perp + np.sin(theta) * perp2)
point = arrow_start + direction * t + offset
points.append(point)
colors.append(color)
return np.array(points), np.array(colors)
# ============================================================================
# RKNN推理
# ============================================================================
def rknn_infer(rknn_model, bgr_origin, target_class_id=None):
h_ori, w_ori = bgr_origin.shape[:2]
W, H = RKNN_INPUT_SIZE
img_resize = cv2.resize(bgr_origin, (W, H))
img_rgb = cv2.cvtColor(img_resize, cv2.COLOR_BGR2RGB)
input_data = np.expand_dims(img_rgb, axis=0).astype(np.uint8)
outs = rknn_model.inference([input_data])
raw_det = outs[0][0]
proto = outs[1][0]
xywh_all = raw_det[:4, :].T
cls_logits = raw_det[4:5, :].T
mask_coeff_all = raw_det[5:37, :].T
conf_all = np.max(sigmoid(cls_logits), axis=1)
cls_id_all = np.argmax(cls_logits, axis=1)
if target_class_id is not None:
class_mask = (cls_id_all == target_class_id)
conf_filtered = conf_all * class_mask.astype(np.float32)
else:
conf_filtered = conf_all
valid_mask = conf_filtered > RKNN_CONF_THRESH
xywh_valid = xywh_all[valid_mask]
conf_valid = conf_filtered[valid_mask]
mask_coeff_valid = mask_coeff_all[valid_mask]
if len(xywh_valid) == 0:
return None, None, False
xyxy_valid = xywh2xyxy(xywh_valid)
valid_box_mask = ~((xyxy_valid[:, 0] >= W) | (xyxy_valid[:, 1] >= H) |
(xyxy_valid[:, 2] <= 0) | (xyxy_valid[:, 3] <= 0))
xyxy_valid = xyxy_valid[valid_box_mask]
conf_valid = conf_valid[valid_box_mask]
mask_coeff_valid = mask_coeff_valid[valid_box_mask]
if len(xyxy_valid) == 0:
return None, None, False
nms_idx = nms_cv2(xyxy_valid, conf_valid, RKNN_CONF_THRESH, RKNN_IOU_THRESH)
if len(nms_idx) == 0:
return None, None, False
best_idx = np.argmax(conf_valid[nms_idx])
final_idx = nms_idx[best_idx]
x1, y1, x2, y2 = xyxy_valid[final_idx].astype(int)
coeff = mask_coeff_valid[final_idx]
mask_pred = np.matmul(coeff[np.newaxis, :], proto.reshape(32, -1))[0]
mask_pred = mask_pred.reshape(160, 160)
mask_pred = sigmoid(mask_pred)
mask_bin = (mask_pred > 0.5).astype(np.uint8)
mask_resized = cv2.resize(mask_bin, (W, H), interpolation=cv2.INTER_LINEAR)
box_mask = np.zeros_like(mask_resized)
x1, y1, x2, y2 = np.clip([x1, y1, x2, y2], 0, W - 1)
box_mask[y1:y2, x1:x2] = 1
mask_final = np.logical_and(mask_resized, box_mask).astype(np.uint8)
mask_origin = cv2.resize(mask_final, (w_ori, h_ori), interpolation=cv2.INTER_LINEAR)
bbox_origin = np.array([x1 / W * w_ori, y1 / H * h_ori, x2 / W * w_ori, y2 / H * h_ori]).astype(int)
return bbox_origin, mask_origin, True
# ==========================
# 🔑 智能轴检测函数 - 核心新增功能
# ==========================
def detect_box_axes_intelligently(points, plane_model, box_length, box_width, box_height, tolerance=0.03):
"""
智能检测盒子的三个轴方向
"""
# 提取法向量作为Z轴候选
[a, b, c, d] = plane_model
z_axis_candidate = np.array([a, b, c])
z_axis_candidate = z_axis_candidate / (np.linalg.norm(z_axis_candidate) + 1e-8)
# 确保Z轴朝上（如果法向量有垂直分量）
if abs(z_axis_candidate[2]) > 0.1:
if z_axis_candidate[2] < 0:
z_axis_candidate = -z_axis_candidate
# 投影点云到平面
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis_candidate), z_axis_candidate)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis_candidate)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis_candidate) * z_axis_candidate
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis_candidate, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
# 投影到2D
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
# 计算凸包
if len(points_2d) < 4:
return None, None, None, None
hull = ConvexHull(points_2d)
hull_vertices = points_2d[hull.vertices]
# 提取凸包的边长
edge_lengths_2d = []
edge_dirs_2d = []
for i in range(len(hull_vertices)):
j = (i + 1) % len(hull_vertices)
edge_vec = hull_vertices[j] - hull_vertices[i]
edge_len = np.linalg.norm(edge_vec)
if edge_len > 0.001:
edge_lengths_2d.append(edge_len)
edge_dirs_2d.append(edge_vec / edge_len)
# 如果边太少，无法判断
if len(edge_lengths_2d) < 2:
return None, None, None, None
# 找到最长边和最短边
sorted_indices = np.argsort(edge_lengths_2d)[::-1]
longest_edge_len = edge_lengths_2d[sorted_indices[0]]
shortest_edge_len = edge_lengths_2d[sorted_indices[-1]]
longest_dir = edge_dirs_2d[sorted_indices[0]]
shortest_dir = edge_dirs_2d[sorted_indices[-1]]
# 构建可能的匹配组合
possible_faces = []
# 顶面: 长x宽
if abs(longest_edge_len - box_length) < tolerance and abs(shortest_edge_len - box_width) < tolerance:
possible_faces.append(('top', box_length, box_width, box_height))
if abs(longest_edge_len - box_width) < tolerance and abs(shortest_edge_len - box_length) < tolerance:
possible_faces.append(('top', box_width, box_length, box_height))
# 侧面1: 长x高
if abs(longest_edge_len - box_length) < tolerance and abs(shortest_edge_len - box_height) < tolerance:
possible_faces.append(('side_lh', box_length, box_height, box_width))
if abs(longest_edge_len - box_height) < tolerance and abs(shortest_edge_len - box_length) < tolerance:
possible_faces.append(('side_lh', box_height, box_length, box_width))
# 侧面2: 宽x高
if abs(longest_edge_len - box_width) < tolerance and abs(shortest_edge_len - box_height) < tolerance:
possible_faces.append(('side_wh', box_width, box_height, box_length))
if abs(longest_edge_len - box_height) < tolerance and abs(shortest_edge_len - box_width) < tolerance:
possible_faces.append(('side_wh', box_height, box_width, box_length))
# 如果没有匹配，尝试更宽松的匹配或使用默认
if not possible_faces:
print("⚠️ 尺寸匹配失败，使用默认顶面假设")
if box_length >= box_width:
possible_faces.append(('top', box_length, box_width, box_height))
else:
possible_faces.append(('top', box_width, box_length, box_height))
# 选择第一个匹配的
detected_face, size1, size2, size_z = possible_faces[0]
print(f"🔍 检测到面: {detected_face}")
print(f" 平面内边长1: {longest_edge_len:.4f}m -> 匹配 {size1:.4f}m")
print(f" 平面内边长2: {shortest_edge_len:.4f}m -> 匹配 {size2:.4f}m")
print(f" 法向量方向尺寸: {size_z:.4f}m")
# 确定轴方向
z_axis = z_axis_candidate.copy()
x_axis_2d = longest_dir
x_axis = x_axis_2d[0] * x_temp + x_axis_2d[1] * y_temp
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis_2d = shortest_dir
y_axis = y_axis_2d[0] * x_temp + y_axis_2d[1] * y_temp
y_axis = y_axis - np.dot(y_axis, x_axis) * x_axis
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
cross_xy = np.cross(x_axis, y_axis)
if np.dot(cross_xy, z_axis) < 0:
y_axis = -y_axis
print("🔄 翻转Y轴以保持右手坐标系")
y_axis = y_axis - np.dot(y_axis, z_axis) * z_axis
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
if detected_face.startswith('side'):
if z_axis[2] < 0:
z_axis = -z_axis
x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
print(f"📐 最终轴方向:")
print(f" X轴: ({x_axis[0]:.4f}, {x_axis[1]:.4f}, {x_axis[2]:.4f})")
print(f" Y轴: ({y_axis[0]:.4f}, {y_axis[1]:.4f}, {y_axis[2]:.4f})")
print(f" Z轴: ({z_axis[0]:.4f}, {z_axis[1]:.4f}, {z_axis[2]:.4f})")
return x_axis, y_axis, z_axis, detected_face
# ============================================================================
# 主节点
# ============================================================================
class CameraDetectionNode(Node):
def __init__(self):
super().__init__('ros2_camera_detection')
self.bridge = CvBridge()
self.rgb_img = None
self.depth_img = None
self.K = None
self.depth_K = None
self.last_process_time = 0
self.latest_points_3d = None
self.latest_colors = None
self.latest_pose_6d = None
self.latest_inlier_mask = None
self.pipeline_lock = threading.Lock()
# 创建点云保存目录
if ENABLE_SAVE_PCD:
os.makedirs(PCD_SAVE_DIR, exist_ok=True)
self.get_logger().info(f"📁 点云保存目录: {PCD_SAVE_DIR}")
self.get_logger().info(f"📁 点云文件名: {PCD_FILENAME} (每次覆盖)")
# 加载RKNN
self.load_rknn_model()
# 订阅相机数据
self.create_subscription(
CameraDriver,
"/vision/camera_data",
self.camera_data_callback,
QoSPresetProfiles.SYSTEM_DEFAULT.value
)
# 发布话题
self.pointcloud_pub = self.create_publisher(PointCloud2, POINTCLOUD_TOPIC, 10)
self.result_image_pub = self.create_publisher(Image, RESULT_IMAGE_TOPIC, 10)
self.final_pose_pub = self.create_publisher(PoseStamped, FINAL_POSE_TOPIC, 10)
# Action Server
self.action_server = ActionServer(
node=self,
action_name=ACTION_NAME,
action_type=VisionDetection,
execute_callback=self.execute_callback,
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback,
)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 简化版位姿估计节点启动 (仅末端坐标系)")
self.get_logger().info(f"📤 发布话题: {FINAL_POSE_TOPIC}")
if ENABLE_SAVE_PCD:
self.get_logger().info(f"💾 点云保存已启用: {PCD_SAVE_DIR}/{PCD_FILENAME} (每次覆盖)")
self.get_logger().info("=" * 60)
def load_rknn_model(self):
try:
self.rknn = RKNNLite()
ret = self.rknn.load_rknn(RKNN_MODEL_PATH)
if ret != 0:
self.get_logger().error(f"❌ RKNN模型加载失败")
self.rknn = None
return
ret = self.rknn.init_runtime()
if ret != 0:
self.get_logger().error(f"❌ RKNN运行时初始化失败")
self.rknn.release()
self.rknn = None
return
self.get_logger().info(f"✅ RKNN模型加载成功")
except Exception as e:
self.get_logger().error(f"❌ RKNN初始化异常: {e}")
self.rknn = None
def camera_data_callback(self, msg: CameraDriver):
try:
self.rgb_img = self.bridge.imgmsg_to_cv2(msg.rgb, 'bgr8')
depth_msg = msg.depth
if depth_msg.encoding == "16UC1":
self.depth_img = np.ndarray(
shape=(depth_msg.height, depth_msg.width),
dtype=np.uint16,
buffer=depth_msg.data
)
else:
return
self.K = np.array(msg.rgb_camera_info.k).reshape(3, 3)
if hasattr(msg, 'depth_camera_info') and msg.depth_camera_info:
self.depth_K = np.array(msg.depth_camera_info.k).reshape(3, 3)
else:
self.depth_K = self.K.copy()
self.process()
except Exception as e:
self.get_logger().error(f"回调错误: {e}")
def process(self):
if self.rgb_img is None or self.depth_img is None or self.K is None:
return
if self.rknn is None:
return
current_time = time.time()
if current_time - self.last_process_time < PROCESS_INTERVAL:
return
self.last_process_time = current_time
with self.pipeline_lock:
start = time.time()
rgb = self.rgb_img.copy()
depth = self.depth_img.astype(np.float32) / DEPTH_SCALE
depth[np.isnan(depth)] = 0
depth[np.isinf(depth)] = 0
depth[(depth < DEPTH_MIN) | (depth > DEPTH_MAX)] = 0
# 1. 目标检测
bbox, mask, success = rknn_infer(self.rknn, rgb, target_class_id=TARGET_CLASS_ID)
if not success:
self.get_logger().warning("⚠️ 未检测到目标")
return
end1 = time.time()
# 形态学操作
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))
mask = cv2.dilate(mask, kernel, iterations=MORPH_DILATE_ITERATIONS)
mask_pixels = np.sum(mask)
if mask_pixels < MIN_MASK_PIXELS:
self.get_logger().warning(f"⚠️ 掩码像素不足: {mask_pixels}")
return
end2 = time.time()
# 2. 生成点云
points_3d, colors = self.generate_pointcloud(rgb, depth, mask)
if points_3d is None or len(points_3d) <= 0:
return
end3 = time.time()
# 3. RANSAC平面分割
plane_model, inlier_cloud, outlier_cloud = self.ransac_plane(points_3d)
if plane_model is None:
return
end4 = time.time()
# 获取内点索引
inlier_indices = np.asarray(inlier_cloud.points) if inlier_cloud is not None else np.array([])
# 创建内点掩码
inlier_mask = np.zeros(len(points_3d), dtype=bool)
if len(inlier_indices) > 0:
from scipy.spatial import KDTree
tree = KDTree(points_3d)
for inlier_pt in inlier_indices:
dist, idx = tree.query(inlier_pt)
if dist < 0.001:
inlier_mask[idx] = True
end5 = time.time()
# 4. 计算6D位姿 (相机坐标系)
pose_camera_6d, axes_dict = self.estimate_6d_pose(plane_model, inlier_cloud)
if pose_camera_6d is None:
self.get_logger().error("❌ 位姿估计失败")
return
# 5. 转换到末端坐标系 (手眼标定)
pose_gripper_6d = self.transform_camera_to_gripper(pose_camera_6d)
end6 = time.time()
# 保存最新数据
self.latest_points_3d = points_3d
self.latest_colors = colors
self.latest_pose_6d = pose_gripper_6d
self.latest_inlier_mask = inlier_mask
# 6. 发布结果
self.publish_result_image(rgb, mask, bbox)
self.publish_pointcloud(points_3d, colors)
self.publish_final_pose(pose_gripper_6d)
end7 = time.time()
# 7. 保存点云数据（使用固定文件名，覆盖模式）
if ENABLE_SAVE_PCD and axes_dict is not None:
try:
# 获取轴信息（相机坐标系）
x_axis = axes_dict['x_axis']
y_axis = axes_dict['y_axis']
z_axis = axes_dict['z_axis']
origin = axes_dict['origin']
# 保存相机坐标系下的点云
save_combined_pointcloud(
all_points=points_3d,
all_colors=colors,
inlier_mask=inlier_mask,
pose_center=origin,
x_axis=x_axis,
y_axis=y_axis,
z_axis=z_axis,
timestamp=current_time,
save_dir=PCD_SAVE_DIR
)
# ============================================================
# 🔑 新增：保存末端坐标系下的点云
# ============================================================
# 转换点云到末端坐标系
points_3d_gripper = self.transform_points_to_gripper(points_3d)
# 获取末端坐标系下的位姿（已经在 transform_camera_to_gripper 中计算）
pose_gripper_6d = self.latest_pose_6d
# 从 pose_gripper_6d 提取轴方向
T_gripper = pose_6d_to_matrix(pose_gripper_6d)
x_axis_gripper = T_gripper[:3, 0]
y_axis_gripper = T_gripper[:3, 1]
z_axis_gripper = T_gripper[:3, 2]
origin_gripper = T_gripper[:3, 3]
# 保存末端坐标系下的点云
save_combined_pointcloud_gripper(
all_points=points_3d_gripper,
all_colors=colors,
inlier_mask=inlier_mask,
pose_center=origin_gripper,
x_axis=x_axis_gripper,
y_axis=y_axis_gripper,
z_axis=z_axis_gripper,
timestamp=current_time,
save_dir=PCD_SAVE_DIR
)
except Exception as e:
self.get_logger().error(f"❌ 保存点云失败: {e}")
# 打印结果
self.print_results(pose_camera_6d, pose_gripper_6d)
end8 = time.time()
elapsed = time.time() - start
self.get_logger().info(f"⏱️ 1,目标检测: {(end1 - start) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 2.形态学操作: {(end2 - end1) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 3.点云生成: {(end3 - end2) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 4.平面分割: {(end4 - end3) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 5.获取内点索引: {(end5 - end4) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 6.姿态计算耗时: {(end6 - end5) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 7.发布结果: {(end7 - end6) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 8.保存点云数据: {(end8 - end7) * 1000:.1f}ms")
self.get_logger().info(f"⏱️ 9.处理总耗时: {elapsed * 1000:.1f}ms")
def generate_pointcloud(self, rgb, depth, mask):
h, w = rgb.shape[:2]
# fx = self.depth_K[0, 0]
# fy = self.depth_K[1, 1]
# cx = self.depth_K[0, 2]
# cy = self.depth_K[1, 2]
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
ys, xs = np.where(mask > 0)
if len(ys) < MIN_MASK_PIXELS:
return None, None
points_3d = []
colors = []
for v, u in zip(ys, xs):
z = depth[v, u] + camera_pingyi_z
if z <= 0:
continue
x = (u - cx) * z / fx - camera_pingyi_x
y = (v - cy) * z / fy
b, g, r = rgb[v, u]
points_3d.append([x, y, z])
colors.append([r, g, b])
if len(points_3d) == 0:
return None, None
points_3d = np.array(points_3d, dtype=np.float32)
colors = np.array(colors, dtype=np.uint8)
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
return points_3d, colors
def ransac_plane(self, points):
if len(points) < 10:
return None, None, None
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
plane_model, inliers = pcd.segment_plane(
RANSAC_DISTANCE_THRESHOLD, RANSAC_N, RANSAC_ITERATIONS
)
if len(inliers) < 10:
return None, None, None
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)
return plane_model, inlier_cloud, outlier_cloud
def estimate_6d_pose(self, plane_model, inlier_cloud):
points = np.asarray(inlier_cloud.points)
if len(points) < 10:
return None, None
# 智能轴检测
x_axis, y_axis, z_axis, detected_face = detect_box_axes_intelligently(
points, plane_model,
BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT,
tolerance=SIZE_MATCH_TOLERANCE
)
if x_axis is None or y_axis is None or z_axis is None:
# 回退到PCA方法
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if z_axis[2] < 0:
z_axis = -z_axis
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
detected_face = 'unknown'
# ============================================================
# 🔑 绕X轴旋转180度
# 变换：X -> X, Y -> -Y, Z -> -Z
# 结果：X朝右，Y朝前，Z朝下
# ============================================================
# 构建当前旋转矩阵
R_current = np.column_stack([x_axis, y_axis, z_axis])
# 绕X轴旋转180度的旋转矩阵
R_transform = np.array([
[1, 0, 0],
[0, -1, 0],
[0, 0, -1]
])
# 应用变换：R_new = R_transform @ R_current
R_new = R_transform @ R_current
# 提取新的轴
x_new = R_new[:, 0]
y_new = R_new[:, 1]
z_new = R_new[:, 2]
# 确保X轴朝右（x分量为正）
if x_new[0] > 0:
x_new = -x_new
# y_new = -y_new
# z_new = -z_new
# 确保Y轴朝前（y分量为正）
if y_new[1] > 0:
y_new = -y_new
# x_new = -x_new
# z_new = -z_new
# 确保Z轴朝下（z分量为负）
if z_new[2] > 0:
z_new = -z_new
# x_new = -x_new
# y_new = -y_new
# 重新正交化确保是有效的旋转矩阵
rotation_matrix = np.column_stack([x_new, y_new, z_new])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
# 位置：平面中心
center = np.mean(points, axis=0)
# 提取6D位姿
roll, pitch, yaw = matrix_to_euler(rotation_matrix)
pose_6d = [center[0], center[1], center[2], roll, pitch, yaw]
axes_dict = {
'x_axis': rotation_matrix[:, 0],
'y_axis': rotation_matrix[:, 1],
'z_axis': rotation_matrix[:, 2],
'origin': center,
'detected_face': detected_face
}
print(f"📐 调整后的轴方向 (相机坐标系):")
print(f" X轴 (朝右): ({rotation_matrix[0, 0]:.4f}, {rotation_matrix[1, 0]:.4f}, {rotation_matrix[2, 0]:.4f})")
print(f" Y轴 (朝前): ({rotation_matrix[0, 1]:.4f}, {rotation_matrix[1, 1]:.4f}, {rotation_matrix[2, 1]:.4f})")
print(f" Z轴 (朝下): ({rotation_matrix[0, 2]:.4f}, {rotation_matrix[1, 2]:.4f}, {rotation_matrix[2, 2]:.4f})")
return pose_6d, axes_dict
def transform_camera_to_gripper(self, pose_camera_6d):
"""转换到手眼标定后的末端坐标系"""
T_camera = pose_6d_to_matrix(pose_camera_6d)
T_gripper = HAND_EYE_MATRIX @ T_camera
pos = T_gripper[:3, 3]
roll, pitch, yaw = matrix_to_euler(T_gripper[:3, :3])
return [pos[0], pos[1], pos[2], roll, pitch, yaw]
def transform_points_to_gripper(self, points_camera):
"""将相机坐标系下的点云转换到末端坐标系"""
if len(points_camera) == 0:
return points_camera
try:
# 构建齐次坐标
ones = np.ones((len(points_camera), 1))
points_homogeneous = np.hstack([points_camera, ones])
# 应用手眼标定矩阵
points_gripper = (HAND_EYE_MATRIX @ points_homogeneous.T).T
return points_gripper[:, :3]
except Exception as e:
self.get_logger().error(f"点云转换失败: {e}")
return points_camera
def publish_result_image(self, rgb, mask, bbox):
try:
annotated = rgb.copy()
if mask is not None and np.sum(mask) > 0:
mask_vis = mask.astype(np.uint8) * 255
mask_colored = np.zeros_like(rgb)
mask_colored[:, :, 1] = mask_vis
annotated = cv2.addWeighted(rgb, 0.7, mask_colored, 0.3, 0)
if bbox is not None:
x1, y1, x2, y2 = bbox
cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.putText(annotated, "Target", (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
msg = self.bridge.cv2_to_imgmsg(annotated, "bgr8")
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "camera_link"
self.result_image_pub.publish(msg)
except Exception as e:
self.get_logger().error(f"发布图像失败: {e}")
def publish_pointcloud(self, points_3d, colors):
if len(points_3d) == 0:
return
try:
cloud_msg = create_point_cloud(points_3d, colors, "camera_link", self.get_clock())
self.pointcloud_pub.publish(cloud_msg)
except Exception as e:
self.get_logger().error(f"发布点云失败: {e}")
def publish_final_pose(self, pose_gripper_6d):
if pose_gripper_6d is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "gripper_link"
pose_msg.pose.position.x = float(pose_gripper_6d[0])
pose_msg.pose.position.y = float(pose_gripper_6d[1])
pose_msg.pose.position.z = float(pose_gripper_6d[2])
quat = euler_zyx_to_quaternion(pose_gripper_6d[3], pose_gripper_6d[4], pose_gripper_6d[5])
pose_msg.pose.orientation.w = quat[0]
pose_msg.pose.orientation.x = quat[1]
pose_msg.pose.orientation.y = quat[2]
pose_msg.pose.orientation.z = quat[3]
self.final_pose_pub.publish(pose_msg)
self.get_logger().info(
f"📤 发布末端位姿: ({pose_gripper_6d[0] * 1000:.1f}, {pose_gripper_6d[1] * 1000:.1f}, {pose_gripper_6d[2] * 1000:.1f}) mm")
def print_results(self, pose_camera, pose_gripper):
self.get_logger().info("=" * 60)
self.get_logger().info("🎯 6D位姿估计结果")
self.get_logger().info("-" * 60)
roll_cam, pitch_cam, yaw_cam = np.degrees(pose_camera[3:6])
self.get_logger().info(f"📷 相机坐标系:")
self.get_logger().info(
f" 位置: ({pose_camera[0] * 1000:.1f}, {pose_camera[1] * 1000:.1f}, {pose_camera[2] * 1000:.1f}) mm")
self.get_logger().info(f" 姿态: ({roll_cam:.2f}°, {pitch_cam:.2f}°, {yaw_cam:.2f}°)")
roll_gri, pitch_gri, yaw_gri = np.degrees(pose_gripper[3:6])
self.get_logger().info(f"🔧 末端坐标系 (手眼标定后):")
self.get_logger().info(
f" 位置: ({pose_gripper[0] * 1000:.1f}, {pose_gripper[1] * 1000:.1f}, {pose_gripper[2] * 1000:.1f}) mm")
self.get_logger().info(f" 姿态: ({roll_gri:.2f}°, {pitch_gri:.2f}°, {yaw_gri:.2f}°)")
self.get_logger().info("=" * 60)
def goal_callback(self, goal_request):
self.get_logger().info(f"📥 收到目标请求: {goal_request.target_obj_name}")
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle):
self.get_logger().info("❌ 任务取消")
return CancelResponse.ACCEPT
def execute_callback(self, goal_handle: ServerGoalHandle):
self.get_logger().info("=" * 60)
self.get_logger().info("🔴 EXECUTE_CALLBACK 被调用")
self.get_logger().info("=" * 60)
goal = goal_handle.request
result = VisionDetection.Result()
try:
feedback = VisionDetection.Feedback()
feedback.task_status = "Processing..."
feedback.progress_rate = 0.0
feedback.current_step_info = "Initializing..."
points_3d = self.latest_points_3d
pose = self.latest_pose_6d
if points_3d is None or len(points_3d) == 0:
self.get_logger().error("❌ 没有可用数据")
result.success = False
result.status_code = 1
result.error_message = "没有可用的检测结果"
goal_handle.abort()
return result
feedback.progress_rate = 0.3
feedback.current_step_info = "Processing 6D pose..."
goal_handle.publish_feedback(feedback)
result.success = True
result.status_code = 0
result.error_message = ""
result.execution_time = time.time() - self.last_process_time
result.header = Header()
result.header.stamp = self.get_clock().now().to_msg()
result.header.frame_id = "gripper_link"
if goal.need_6d_pose:
feedback.progress_rate = 0.6
feedback.current_step_info = "Creating pose message..."
goal_handle.publish_feedback(feedback)
result.target_6d_pose = PoseStamped()
result.target_6d_pose.header = Header()
result.target_6d_pose.header.stamp = self.get_clock().now().to_msg()
result.target_6d_pose.header.frame_id = "gripper_link"
if pose is not None:
result.target_6d_pose.pose.position.x = float(pose[0])
result.target_6d_pose.pose.position.y = float(pose[1])
result.target_6d_pose.pose.position.z = float(pose[2])
quat = euler_zyx_to_quaternion(pose[3], pose[4], pose[5])
result.target_6d_pose.pose.orientation.w = quat[0]
result.target_6d_pose.pose.orientation.x = quat[1]
result.target_6d_pose.pose.orientation.y = quat[2]
result.target_6d_pose.pose.orientation.z = quat[3]
result.pose_confidence = 0.95
else:
centroid = np.mean(points_3d, axis=0)
result.target_6d_pose.pose.position.x = float(centroid[0])
result.target_6d_pose.pose.position.y = float(centroid[1])
result.target_6d_pose.pose.position.z = float(centroid[2])
result.target_6d_pose.pose.orientation.w = 1.0
result.pose_confidence = 0.5
self.get_logger().warn("⚠️ 使用降级方案")
if goal.need_env_point_cloud:
feedback.progress_rate = 0.8
feedback.current_step_info = "Creating point cloud..."
goal_handle.publish_feedback(feedback)
result.env_point_cloud = create_point_cloud(
points_3d,
self.latest_colors if self.latest_colors is not None else np.ones((len(points_3d), 3),
dtype=np.uint8) * 255,
"camera_link",
self.get_clock()
)
result.point_cloud_frame_id = "camera_link"
self.get_logger().info(f"☁️ 点云已创建，点数: {len(points_3d)}")
feedback.progress_rate = 1.0
feedback.task_status = "Completed"
feedback.current_step_info = "Done"
goal_handle.publish_feedback(feedback)
self.get_logger().info("✅ 返回结果")
goal_handle.succeed()
return result
except Exception as e:
self.get_logger().error(f"❌ execute_callback 异常: {e}")
import traceback
traceback.print_exc()
result.success = False
result.status_code = 3
result.error_message = str(e)
goal_handle.abort()
return result
def __del__(self):
cv2.destroyAllWindows()
def main(args=None):
rclpy.init(args=args)
node = CameraDetectionNode()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("接收到退出信号")
finally:
cv2.destroyAllWindows()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
5
0- 0
已为社区贡献7条内容
所有评论(0)