ROS D435I识别目标并获取深度数据

ROS D435I识别目标并获取深度数据

使用D435I相机，并基于ros获取到彩色图像和匹配后的深度数据，通过OPENCV对彩色图像进行目标识别，得到目标所在的像素范围，随后得到深度数据
重点在于：转换ros图像数据到opencv格式，得到目标像素点的实际深度值

1. d435i启动与修改

roslaunch realsense2_camera rs_camera_vins.launch 

使用上述指令启动d435i，可以在里面进行分辨率修改和匹配。其中color_width和color_height是彩色图像的分辨率，可以选择720480、1280720和1920*1080。修改align_depth进行深度匹配，得到匹配后的深度图像。

rqt_image_view

使用上述指令查看图像，可以看到需要分辨率的彩色图像和深得图像。其中深度图像可以看出，距离很近或者很远的时候，深度数据是黑色的。彩色图像是bgr8的格式，深度图像是16UC1格式，可以通过Image.encoding获得。

rostopic echo /camera/aligned_depth_to_color/camera_info

使用上述指令，可以查看图像的参数，包括分辨率和内参。

2. ros图像转转为opencv进行处理

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

rospy.init_node('vins_img')
rospy.Subscriber("/camera/color/image_raw",                                         Image, vins_img_callback,queue_size=1)
bridge = CvBridge()
image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

def vins_img_callback(data = Image):
    try:
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
        print(e)
        
final_img = ……

    try:
        image_pub.publish(bridge.cv2_to_imgmsg(final_img, "bgr8"))
    except CvBridgeError as e:
        print(e)

    pass

while not rospy.is_shutdown():
    rospy.spin()

使用上述代码，可以将获取的图像转为三通道bgr的opencv图像，可以直接用opencv进行处理，（但是我出现了无法cv2.imshow），发布后的图像可以用rqt看到。

3. 获取深度实际值
   前面说到深度值是的格式是16UC1，转化成opencv后，直接通过数组得到对应的值即可，单位是mm。

def vins_dep_callback(data = Image):
    global depth_img
    try:
        depth_img = bridge.imgmsg_to_cv2(data, "16UC1")
    except CvBridgeError as e:
        print(e)

上述代码将深度图形转换为opencv的格式，并且是16UC1的编码方式。读取到他的shape分别为720和1280，通过print(depth_img[depth_cy, depth_cx])即可获得深度值。

4. 发布自己的topic，进行ros之间的传输
   自己造一个topic的数据结构太麻烦了，还是使用现有的，使用from geometry_msgs.msg import Point，这个有x y z的数据，正好用于表示。
   有两种方案，xy表示图像中的目标像素坐标或者转换之后的pitch和yaw偏执，z表示深度数据；或者左前上的坐标顺序，x轴表示yaw的偏执，z表示pitch的偏执（之后会转换成高度的偏执），y表示深度数据。
   在此使用第二个方案

from geometry_msgs.msg import Point
obj_pub = rospy.Publisher("obj_point",  Point, queue_size=1 )

    obj_dat  = Point()
    obj_dat.x = det_yaw
    obj_dat.y = det_pitch
    obj_dat.z = float(depth_img[depth_cy, depth_cx])/1000
    obj_pub.publish(obj_dat)