基于RealSense D435相机简单实现手部姿态重定向

基于Intel RealSense D435 相机和 MediaPipe的手部姿态检测，进一步简单实现手部姿态与机器人末端的重定向。
假设已经按照【基于 RealSenseD435i相机实现手部姿态检测】配置好所需的库和环境，并且有一个可以控制的机器人接口。

一、手部姿态重定向介绍

手部姿态重定向通常涉及将实时手部关键点映射到虚拟环境或另一个坐标系中（通常需要映射到机器人坐标系中）。可以使用以下步骤实现基本的手部姿态重定向：

1. 获取关键点坐标：使用手部追踪库（如 MediaPipe）获取手部关键点的坐标。
   具体包括数据获取和手部特征识别。
   首先选择合适的相机，如RGB相机、深度相机（如Kinect或RealSense），或高帧率摄像头获取实时图像或深度数据；
   然后使用机器学习或深度学习算法（如YOLO、SSD等）检测图像中的手部，也可以使用现成的手部检测模型，例如MediaPipe Hands，来实现实时手部跟踪；
   最后提取手部的关键点信息，例如手指关节、掌心等。

2. 定义目标坐标系：确定将手部姿态映射到哪个坐标系中，比如虚拟现实环境或者机器人坐标系。

3. 姿态重定向：根据目标坐标系的需求，进行平移、旋转或缩放等变换。
   首先将2D图像坐标转换为3D空间坐标，通常需要相机内参（焦距、主点位置等），根据关键点位置计算手部的姿态（位置、方向、旋转）；
   然后可以使用旋转矩阵、四元数等方式表示手部的姿态；最后将手部的当前姿态转换到机器人的目标姿态。

4. 输出重定向后的姿态：将重定向后的坐标记录用于后续处理。

二、简单实现手部姿态与机器人末端的重定向

MediaPipe检测器可以准确定位腕部框架中21个手指关节坐标的3D关键点和图像上的2D关键点。

代码示例

import cv2
import numpy as np
import pyrealsense2 as rs
import mediapipe as mp
import time# 初始化 RealSense 管道
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
pipeline.start(config)# 初始化 MediaPipe 手部模块
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(max_num_hands=2, min_detection_confidence=0.7)
mp_draw = mp.solutions.drawing_utils# 机器人控制函数（示例），具体需要根据需要通过逆解解算
def move_robot_to(position):# 在这里添加机器人控制代码print(f"移动机器人末端到位置: {position}")try:while True:# 获取图像帧frames = pipeline.wait_for_frames()color_frame = frames.get_color_frame()if not color_frame:continue# 转换为 numpy 数组color_image = np.asanyarray(color_frame.get_data())# 将图像转换为 RGB 格式rgb_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)rgb_image.flags.writeable = False# 检测手部results = hands.process(rgb_image)# 将图像转换回 BGR 格式rgb_image.flags.writeable = Trueannotated_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)# 如果检测到手部if results.multi_hand_landmarks:for hand_landmarks in results.multi_hand_landmarks:# 绘制手部标记mp_draw.draw_landmarks(annotated_image, hand_landmarks, mp_hands.HAND_CONNECTIONS)# 获取手腕的位置（关节0）wrist = hand_landmarks.landmark[mp_hands.HandLandmark.WRIST]h, w, c = annotated_image.shapewrist_x, wrist_y = int(wrist.x * w), int(wrist.y * h)# 将手腕位置转换为机器人坐标，需要进行不同坐标系的位姿变换robot_position = (wrist_x, wrist_y)move_robot_to(robot_position)  # 移动机器人末端# 显示结果cv2.imshow('Hand Detection', annotated_image)if cv2.waitKey(1) & 0xFF == ord('q'):break
finally:# 释放资源pipeline.stop()cv2.destroyAllWindows()

代码说明

1. 初始化 RealSense 管道：设置相机流，并启动管道。
2. 初始化 MediaPipe：配置手部检测模块。
3. 实时捕获与处理：在循环中捕获视频帧，并检测手部姿态。
4. 机器人控制：通过 move_robot_to 函数模拟移动机器人末端到手腕位置，需要根据自己的机器人接口实现具体的控制代码。
5. 显示结果：在窗口中显示手部检测的图像，按 q 键退出。

运行代码

1. 将代码保存为 hand_pose_robot.py。

2. 连接 Intel RealSense D435 相机。

3. 在终端中运行代码：

   python hand_pose_robot.py
   

运行结果

三、总结

确保环境中已经安装了相关库，并且相机正常工作。运行后，窗口中将显示实时的手部检测结果，同时机器人末端会根据手腕位置进行重定向。根据需要调整 move_robot_to 函数的实现，以适配机器人的控制逻辑。