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

一、思路

深度相机会发布一个将深度图像与彩色图像对齐的topic：/camera/aligned_depth_to_color/image_raw（深度流向彩色流对齐），利用该topic可以获取对应的三维坐标，该坐标还需要经过深度相机的内参矩阵变换。

二、变换原理

三、程度实现

3.1 准备工作

1、设置 realsense-ros 的rs_camera.launch文件

说一个这里我自己遇到的坑，realsense默认的分辨率应该是1280×720，我将彩色图像的分辨率改为了640×480，在使用深度图像时没有更改深度图像的分辨率，导致订阅不到这个topic/camera/aligned_depth_to_color/image_raw，所以一定要将彩色图像和深度图像设置相同的分辨率

3.2获取相机的内参矩阵

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)

内参矩阵

K = [fx 0 ppx；0 fy ppy；0 0 1]

3.3 获取三维坐标

#!/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()

注意：在将ROS的图像格式转换为opencv格式时，该选用哪种编码？

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

这里我是查看了原深度图像的编码格式，为16位所以也选用了16位

rostopic echo /camera/aligned_depth_to_color/image_raw/encoding

16位深度图即一个像素值占两个byte，是16位整数，单位是毫米，这也是上面程序中求真实距离时乘0.001的原因（毫米转米）

最后

以上就是无私糖豆为你收集整理的ROS/RealSenseD435i：已知二维图像中一点获取其深度图像中的三维坐标（python实现）的全部内容，希望文章能够帮你解决ROS/RealSenseD435i：已知二维图像中一点获取其深度图像中的三维坐标（python实现）所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。