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

基于Intel RealSense D435相机和MediaPipe的手部姿态检测，进一步简单实现手部姿态与机器人末端的重定向，获取手部的6D坐标（包括位置和姿态）。

假设已经按照【基于 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 = True
        annotated_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.shape
                wrist_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

运行结果

将 Intel RealSense D435 相机与 MediaPipe 的手部姿态检测整合在一起，可以实现手部姿态的实时检测，并将手腕的关键点转换为相机坐标系下的空间坐标。接下来，我们将编写一个完整的示例代码，进行手部关键点坐标到机器人坐标系的转换。

三、简单实现手腕与机器人末端的坐标转换

代码示例

以下是将 Intel RealSense D435 相机与 MediaPipe 手部姿态检测结合的完整代码示例：

import cv2
import mediapipe as mp
import numpy as np
import pyrealsense2 as rs

# 初始化MediaPipe手部模型
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(static_image_mode=False, max_num_hands=1, min_detection_confidence=0.7)
mp_drawing = mp.solutions.drawing_utils

# 设置RealSense相机参数
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# 启动相机流
pipeline.start(config)

# 相机到机器人坐标系的转换矩阵（示例，需要根据实际情况调整）
camera_to_robot_transform = np.array([
    [1, 0, 0, 0],  # x轴
    [0, 1, 0, 0],  # y轴
    [0, 0, 1, 0],  # z轴
    [0, 0, 0, 1]   # 平移
])

def wrist_to_camera_coordinates(wrist_landmark, depth_frame):
    # 获取深度信息
    depth_value = depth_frame.get_distance(int(wrist_landmark.x * 640), int(wrist_landmark.y * 480))
    if depth_value == 0:
        return None  # 如果深度值为0，返回None

    # 计算空间坐标
    x = (wrist_landmark.x * 640 - 320) * depth_value / 525.0  # 525.0是相机焦距（可根据相机参数调整）
    y = (wrist_landmark.y * 480 - 240) * depth_value / 525.0
    z = depth_value

    return np.array([x, y, z])

def main():
    while True:
        # 获取相机帧
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        depth_frame = frames.get_depth_frame()

        if not color_frame or not depth_frame:
            continue
        
        # 转换为numpy数组
        frame = np.asanyarray(color_frame.get_data())

        # RGB转换
        frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        results = hands.process(frame_rgb)

        if results.multi_hand_landmarks:
            for hand_landmarks in results.multi_hand_landmarks:
                # 提取手腕关键点
                wrist_landmark = hand_landmarks.landmark[mp_hands.HandLandmark.WRIST]

                # 转换为相机坐标系下的空间坐标
                wrist_coordinates = wrist_to_camera_coordinates(wrist_landmark, depth_frame)

                if wrist_coordinates is not None:
                    # 映射到机器人坐标系
                    robot_coordinates = camera_to_robot_transform @ np.append(wrist_coordinates, 1)  # 同质坐标转换
                    
                    # 打印机器人坐标
                    print(f"Wrist Coordinates (Camera): {wrist_coordinates}")
                    print(f"Wrist Coordinates (Robot): {robot_coordinates[:3]}")  # 只取x, y, z

                # 可视化手部关键点
                mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)

        cv2.imshow('Hand Tracking', frame)
        
        if cv2.waitKey(1) & 0xFF == 27:  # 按Esc退出
            break

    # 停止相机流
    pipeline.stop()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

代码说明

• RealSense 相机设置 ：使用 pyrealsense2 库来设置和启动 RealSense D435 相机的流。代码配置了颜色流和深度流。

• MediaPipe 手部姿态检测：使用 MediaPipe 检测手部关键点，特别是手腕的位置。

• 手腕坐标转换 ：wrist_to_camera_coordinates 函数根据手腕的关键点和深度图计算相机坐标系下的空间坐标。

• 坐标转换 ：使用转换矩阵将相机坐标系下的手腕坐标转换到机器人坐标系。需要根据实际机器人和相机的位置关系调整 camera_to_robot_transform 矩阵。

• 可视化和输出：代码在每帧中显示检测到的手部关键点，并输出手腕在相机坐标系和机器人坐标系下的坐标。

运行代码

1. 将代码保存为 hand_pose_robot.py。
2. 连接 Intel RealSense D435 相机。
3. 在终端中运行代码：

python hand_pose_robot.py

运行结果

Wrist Coordinates (Camera): [0.04378617 0.0835716  0.58400005]
Wrist Coordinates (Robot): [ 0.04378617 -0.0835716   0.58400005]

四、简单实现手部的6D坐标获取

要获取手部的6D坐标（包括位置和姿态），可以通过以下步骤实现：

1. 获取3D坐标：使用 MediaPipe 提供的手部关键点数据（例如，手腕、手指尖等），获取它们的3D坐标（x, y, z）。

2. 计算姿态信息：姿态信息通常可以通过手部关键点的位置和方向来推断。例如，可以使用手指的相对位置和角度来表示手的朝向。你可以选择特定的关键点（如手腕和食指尖）来定义手的方向。

3. 构造6D坐标：组合位置和姿态信息，构造6D坐标。

代码示例

以下是如何在代码中实现这一点的示例：

import cv2
import numpy as np
import pyrealsense2 as rs
import mediapipe as mp
import math

# 其他部分保持不变...

def calculate_orientation(hand_landmarks):
    # 假设使用手腕和食指尖来计算姿态
    wrist = hand_landmarks.landmark[mp_hands.HandLandmark.WRIST]
    index_finger_tip = hand_landmarks.landmark[mp_hands.HandLandmark.INDEX_FINGER_TIP]

    # 获取3D坐标
    wrist_pos = np.array([wrist.x, wrist.y, wrist.z])
    finger_tip_pos = np.array([index_finger_tip.x, index_finger_tip.y, index_finger_tip.z])

    # 计算手的方向向量
    direction_vector = finger_tip_pos - wrist_pos
    direction_vector /= np.linalg.norm(direction_vector)  # 单位化

    # 将方向向量转换为欧拉角（假设手向上）
    yaw = math.atan2(direction_vector[1], direction_vector[0])
    pitch = math.asin(direction_vector[2])
    roll = 0  # 如果不考虑手的旋转，roll可以设为0

    return wrist_pos, (yaw, pitch, roll)

try:
    while True:
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()

        if not color_frame:
            continue

        color_image = np.asanyarray(color_frame.get_data())
        rgb_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
        rgb_image.flags.writeable = False

        results = hands.process(rgb_image)

        rgb_image.flags.writeable = True
        annotated_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)

                wrist_pos, orientation = calculate_orientation(hand_landmarks)
                robot_position = wrist_pos  # 机器人位置为手腕坐标
                move_robot_to(robot_position)  # 移动机器人末端

                print(f"手腕位置: {wrist_pos}, 姿态: {orientation}")

        cv2.imshow('Hand Detection', annotated_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    pipeline.stop()
    cv2.destroyAllWindows()

代码说明

1. calculate_orientation 函数：计算手腕的3D位置和手的朝向（以欧拉角表示）。
2. 姿态计算：根据手腕和食指尖的位置计算手的方向，并归一化为单位向量，从而得到姿态。
3. 输出结果：打印手腕的位置和姿态。

运行代码

1. 将代码保存为 hand_pose_robot_6D.py。
2. 连接 Intel RealSense D435 相机。
3. 在终端中运行代码：

python hand_pose_robot_6D.py

运行结果

手腕位置: [ 6.16283059e-01  5.05638361e-01 -1.23956696e-07], 姿态: (-1.6486818875015121, -0.3188391517848179, 0)
手腕位置: [ 6.15322232e-01  5.09867549e-01 -1.33983647e-07], 姿态: (-1.6649436814315695, -0.3417587888604621, 0)
手腕位置: [ 6.18688941e-01  5.15018821e-01 -1.20955633e-07], 姿态: (-1.6953785334171043, -0.2905682467378848, 0)
手腕位置: [ 6.19475007e-01  5.10892391e-01 -1.25988706e-07], 姿态: (-1.7442050004258685, -0.17409076245894894, 0)
手腕位置: [ 6.22752011e-01  5.18672347e-01 -9.87428663e-08], 姿态: (-1.8722014318776006, -0.183676503807803, 0)
手腕位置: [6.27165079e-01 5.15416026e-01 8.79372877e-08], 姿态: (-2.0528003434969873, -0.07735958610038009, 0)
手腕位置: [6.26826108e-01 5.08712649e-01 1.98455922e-07], 姿态: (-2.020922761062857, -0.06753396048955623, 0)
手腕位置: [6.31548941e-01 5.09610891e-01 2.30047050e-07], 姿态: (-2.0704836718570623, 0.019604326160985164, 0)
手腕位置: [6.32204950e-01 5.19681990e-01 2.23285795e-07], 姿态: (-2.065844798885916, 0.12585902334658688, 0)
手腕位置: [6.32742882e-01 5.19853532e-01 3.06575970e-07], 姿态: (-2.056895662858401, 0.11631550222837758, 0)

通过这个方法，可以获得6D坐标（位置和姿态），适用于进一步的机器人控制和交互。确保根据具体应用需要对姿态计算进行调整。

五、总结

确保环境中已经安装了相关库，并且相机正常工作。运行后，窗口中将显示实时的手部检测结果，同时机器人末端会根据手腕位置进行重定向。可以实时监测手部动作并将其坐标转换为机器人坐标系，进而实现对机器人的控制。

在实际应用中，可能需要：

• 根据 RealSense 相机的参数调整坐标转换公式。
• 调整机器人坐标系的转换矩阵，以匹配相机与机器人之间的实际位置关系。
• 增强代码的鲁棒性，处理深度数据的缺失情况。