ROS/RealSenseD435i：已知二维图像中一点获取其深度图像中的三维坐标（python实现）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
import pyrealsense2 as rs
import numpy as np
import cv2
import json

pipeline = rs.pipeline()  # 定义流程pipeline
config = rs.config()  # 定义配置config
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)  # 配置depth流
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)  # 配置color流
profile = pipeline.start(config)  # 流程开始
align_to = rs.stream.color  # 与color流对齐
align = rs.align(align_to)
frames = pipeline.wait_for_frames()  # 等待获取图像帧
aligned_frames = align.process(frames)  # 获取对齐帧
aligned_depth_frame = aligned_frames.get_depth_frame()  # 获取对齐帧中的depth帧
color_frame = aligned_frames.get_color_frame()  # 获取对齐帧中的color帧

 ############### 相机参数的获取 #######################
intr = color_frame.profile.as_video_stream_profile().intrinsics  # 获取相机内参
depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics  # 获取深度参数（像素坐标系转相机坐标系会用到）
camera_parameters = {'fx': intr.fx, 'fy': intr.fy,
                         'ppx': intr.ppx, 'ppy': intr.ppy,
                         'height': intr.height, 'width': intr.width,
                         'depth_scale': profile.get_device().first_depth_sensor().get_depth_scale()
                         }
# 保存内参到本地
with open('./intrinsics.json', 'w') as fp:
    json.dump(camera_parameters, fp)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
#!/usr/bin/python
# encoding: utf-8
import rospy
from std_msgs.msg import String
from action_recognition.msg import Torso
from action_recognition.msg import Distance
from sensor_msgs.msg import Image, CameraInfo
import message_filters
import cv2
from cv_bridge import CvBridge, CvBridgeError

def callback(data):
    global bridge
    bridge = CvBridge()

    pub = rospy.Publisher('/Distance', Distance, queue_size=1)
    rate = rospy.Rate(10)
    depth_image = bridge.imgmsg_to_cv2(data, '16UC1')

    # 定义需要得到真实三维信息的像素点（x, y)
    x = 320
    y = 240

    real_z = depth_image[x, y] * 0.001  # mm转换m
    real_x = (x - ppx) / fx * real_z
    real_y = (y - ppy) / fy * real_z

    pub.publish(Distance(real_z))
    cv2.imshow("Image window", depth_image)
    cv2.waitKey(3)


def get_distance():
    rospy.init_node('get_distance', anonymous=True)
    rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, callback)
    rospy.spin()


if __name__ == '__main__':
    global fx, fy, ppx, ppy

    fx = 614.9630737304688
    fy = 615.1746215820312
    ppx = 318.2005920410156
    ppy = 241.51356506347656

    get_distance()

1
2
depth_image = bridge.imgmsg_to_cv2(data, '16UC1')

1
2
rostopic echo /camera/aligned_depth_to_color/image_raw/encoding