ubuntu系统下python直接调用pyrealsense2和mediapipe实现realsense相机3D位姿识别

ubuntu系统下python直接调用pyrealsense2和mediapipe实现realsense相机3D位姿识别

主要分为以下三步：

第一步，调用pyrealsense2库直接读取realsense相机的彩色图和深度图。

第二步，调用mediapipe对读取到的彩色图进行人体骨骼和人脸识别。

第三步，将mediapipe识别到的目标位置信息，即像素宽度x和像素高度y,给到pyrealsense2库里面的
get_3d_camera_coordinate（）函数，该函数会返回该像素点的深度信息和三维坐标信息。

ps:
1.第三步中返回的深度信息有两种理解，第一个就是实际像素点位置与相机中心的距离，另一个就是实际像素点位置到相机中心位置距离在z方向的投影。根据我这边实验和网上资料查找是第一个就两点直接的距离而非距离投影。

2.第三步中返回的三维坐标数据为根据像素位置、相机内参、深度距离计算得出的，感兴趣的也可以利用内参的公式和深度自己算一下，可以参考下面公式。

x = results.pose_landmarks.landmark[0].x*640
Y = results.pose_landmarks.landmark[0].y*480
pos_x = (x-color_intrin.ppx)/color_intrin.fx
pos_y = (y-color_intrin.ppy)/color_intrin.fy

再乘一下深度，就是3D坐标了，可以与返回的3D坐标比较一下，基本是一致的。

3.本案例中用的是mediapipe的识别模型反馈目标的中心像素坐标，基于此我们也可以实现手势人脸识别，或者yolo模型识别等的像素坐标，根据像素坐标我们就可以通过该点的深度信息得出该点的3D坐标。

4.使用3D相机选型的时候，要注意一下相机的深度识别范围，像realsense455只能识别0.3-6m范围内的深度信息，对于小于0.3m距离的深度就没有办法进行识别了。

下面是完整的代码。

#!/usr/bin/python3
# -*- coding: utf-8 -*-

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

mp_drawing = mp.solutions.drawing_utils
mp_drawing_styles = mp.solutions.drawing_styles
# mp_hands = mp.solutions.hands
mp_pose = mp.solutions.pose
''' 
设置
'''
pipeline = rs.pipeline()    # 定义流程pipeline，创建一个管道
config = rs.config()    # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)      # 配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 15)     # 配置color流

pipe_profile = pipeline.start(config)       # streaming流开始

# 创建对齐对象与color流对齐
align_to = rs.stream.color      # align_to 是计划对齐深度帧的流类型
align = rs.align(align_to)      # rs.align 执行深度帧与其他帧的对齐

''' 
获取对齐图像帧与相机参数
'''
def get_aligned_images():
    
    frames = pipeline.wait_for_frames()     # 等待获取图像帧，获取颜色和深度的框架集     
    aligned_frames = align.process(frames)      # 获取对齐帧，将深度框与颜色框对齐  

    aligned_depth_frame = aligned_frames.get_depth_frame()      # 获取对齐帧中的的depth帧 
    aligned_color_frame = aligned_frames.get_color_frame()      # 获取对齐帧中的的color帧

    #### 获取相机参数 ####
    depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics     # 获取深度参数（像素坐标系转相机坐标系会用到）
    color_intrin = aligned_color_frame.profile.as_video_stream_profile().intrinsics     # 获取相机内参


    #### 将images转为numpy arrays ####  
    img_color = np.asanyarray(aligned_color_frame.get_data())       # RGB图  
    img_depth = np.asanyarray(aligned_depth_frame.get_data())       # 深度图（默认16位）

    return color_intrin, depth_intrin, img_color, img_depth, aligned_depth_frame

def get_3d_camera_coordinate(depth_pixel, aligned_depth_frame, depth_intrin):
    x = depth_pixel[0]
    y = depth_pixel[1]
    dis = aligned_depth_frame.get_distance(x, y)        # 获取该像素点对应的深度
    # print ('depth: ',dis)       # 深度单位是m
    camera_coordinate = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, dis)
    print ('camera_coordinate: ',camera_coordinate)
    return dis, camera_coordinate

if __name__=="__main__":
    while True:
        ''' 
        获取对齐图像帧与相机参数
        '''
        color_intrin, depth_intrin, img_color, img_depth, aligned_depth_frame = get_aligned_images()        # 获取对齐图像与相机参数
        #### 在图中标记随机点及其坐标 ####
        # print(color_intrin[1])
        with mp_pose.Pose(min_detection_confidence=0.5,min_tracking_confidence=0.5) as pose:
            image = img_color
            # To improve performance, optionally mark the image as not writeable to
            # pass by reference.
            image = cv2.resize(image, (640,480))
            image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            results = pose.process(image)

            # Draw the hand annotations on the image.
            image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
            
            if results.pose_landmarks:
                if results.pose_landmarks.landmark[0]:
                    print(results.pose_landmarks.landmark[0])
                    x = results.pose_landmarks.landmark[0].x*640
                    y = results.pose_landmarks.landmark[0].y*480
                    pos_x = (x-color_intrin.ppx)/color_intrin.fx
                    pos_y = (y-color_intrin.ppy)/color_intrin.fy
                    
                    x = int(x)
                    y = int(y)
                    cv2.circle(image,(x,y),30,(0,0,255))
                    print(x)
                    print(y)
                    if x > 0 and x < 640 and y > 0 and y < 480 :
                        depth_pixel = [x, y]     # 设置随机点，以相机中心点为例
                        dis, camera_coordinate = get_3d_camera_coordinate(depth_pixel, aligned_depth_frame, depth_intrin)

                        cor_x = str(x-320)
                        cor_y = str(480-y-240)
                        cv2.putText(image,"Dis:"+str(round(dis,2))+" m", (40,40), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[0,0,255])
                        cv2.putText(image,"X:"+cor_x+"pixel", (80,80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
                        cv2.putText(image,"Y:"+cor_y+"pixel", (80,120), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
                        pos_x = round(pos_x*dis,2)
                        pos_y = round(pos_y*dis,2)
                        pos_x = str(pos_x)
                        pos_y = str(-pos_y)

                        cv2.putText(image,"X:"+pos_x+"m", (80,160), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
                        cv2.putText(image,"Y:"+pos_y+"m", (80,200), cv2.FONT_HERSHEY_SIMPLEX, 1.2,[255,0,0])
                        

                        center_point = (320,240)
                        length = 100
                        right_point = (320+length,240)
                        up_point = (320,240-length)
                        cv2.arrowedLine(image, center_point, right_point, (255, 0, 0), 1)
                        cv2.arrowedLine(image, center_point, up_point, (255, 0, 0), 1)
                        # mp_drawing.draw_landmarks(
                        #     image,
                        #     results.pose_landmarks,
                        #     mp_pose.POSE_CONNECTIONS,
                        #     landmark_drawing_spec=mp_drawing_styles.get_default_pose_landmarks_style())
            
        #### 显示画面 ####
        cv2.imshow('RealSence',image)
        key = cv2.waitKey(1)```