python做图像识别,深度图和点云的关系

以英特尔D435I相机为例,您可以使用python代码读取深度图像、RGB图像和点云可视化。

importpyrealsense2asrsimportnumpyasnpimportcv2importpclfrompclimportpcl _ visualization cloud=PCL.point cloud _ pointxyzrgrgd color ) :length=len ) pt ) points=NP.zeros ) ) length,4 ), dtype=np.float32 ) forIinrange(length ) : points [ I ] [0]=pt [ I ] [0] points [ I ] [1]=pt [ I ] [ poin ] NP.uint8(color ) ) cloud.from_array ) points ) )从array构建点云的方式visual_viewer.showcolorcloud ) cloud ) v=true# v=not(visual_viewer.wasstopped ) ) if _ name _=' _ main _ ' : # configuredepthandcolorstreaar config.enation 30 ) config.enable_stream ) RS.stream 30 ) startstreamingpipeline.start (config )深度图像, align _ to _ color=RS.align (RS.stream.color ) PC=RS.polor以颜色排列的visual _ viewer=PCL _ visualization . waitforacoherentpairofframes 3360 depthandcolorframes=pipeline.wait _ for _ frames (frames=align _ to _ color.pra memes ) e=Frames.get_color_frame ) ifno tdepth _ frameornor convertimagestonumpyarraysdepth _ image=NP.asanyarray (深度lycolormapondepthimage (imagemustbeconvertedto8- bitperpixelfirst ) depth _ colormap=cv2.apply colormap ) cv2.conver map cv2.COLORMAP_JET ) stackbothimageshorizontallyimages=NP.h stack ) () color_image,depth _ colormap (# getpointddd 3 ) PC.map_to(color_frame ) points=PC.calculate ) depth_frame )顶点坐标VTX=NP.asanyarray ) points.get_ )

olorful) # Show images cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) cv2.imshow('RealSense', images) key = cv2.waitKey(1) # Press esc or 'q' to close the image window if key & 0xFF == ord('q') or key == 27: cv2.destroyAllWindows() break finally: # Stop streaming pipeline.stop()

若以racord的*.bag文件进行读取和可视化,有两种方案: pipeline = rs.pipeline() # Create a config objectconfig = rs.config() # Tell config that we will use a recorded device from file to be used by the pipeline through playback.rs.config.enable_device_from_file(config, file_path, repeat_playback=False)pipeline.start(config)

其他就和上面用相机读取的时候一致了。 import osimport cv2import numpy as npimport rosbagfrom cv_bridge import CvBridgeimport sensor_msgs.point_cloud2 as pc2from sensor_msgs.msg import PointCloud2import pclfrom pcl import pcl_visualizationimport pyrealsense2 as rsimport numpy as np# 相机参数depth_camera_metrix = np.array([[920.523,0.0,641.936], [0.0, 919.243,351.036], [0,0,1]])def depth2xyz(depth_map,depth_cam_matrix,flatten=False,depth_scale=1000): fx,fy = depth_cam_matrix[0,0],depth_cam_matrix[1,1] cx,cy = depth_cam_matrix[0,2],depth_cam_matrix[1,2] h,w=np.mgrid[0:depth_map.shape[0],0:depth_map.shape[1]] z=depth_map/depth_scale x=(w-cx)*z/fx y=(h-cy)*z/fy xyz=np.dstack((x,y,z)) if flatten==False else np.dstack((x,y,z)).reshape(-1,3) #xyz=cv2.rgbd.depthTo3d(depth_map,depth_cam_matrix) return xyz def visual_pcl(visual, depth, color): xyz = depth2xyz(depth, depth_camera_metrix) # colorful = np.asanyarray(color.get_data()) xyz = xyz.reshape(-1, 3) color=color.reshape(-1,3) # pc.map_to(color) # points = pc.calculate(depth) # vtx = np.asanyarray(points.get_vertices()) cloud = pcl.PointCloud_PointXYZRGB() temp = np.zeros((len(xyz), 4), np.float32) length = len(xyz) for i in range(length): temp[i][0] = xyz[i][0] * 1000.0 temp[i][1] = xyz[i][1] * 1000.0 temp[i][2] = xyz[i][2] * 1000.0 temp[i][3] = color[i][0] cloud.from_array(temp) visual.ShowColorCloud(cloud)if __name__ == "__main__": file_path = "./stairs.bag" visual = pcl_visualization.CloudViewing() pc = rs.pointcloud() points = rs.points() bag = rosbag.Bag(file_path, "r") bag_data = bag.read_messages() info = bag.get_type_and_topic_info() print(info) bridge = CvBridge() depth = None; points_tp = None for topic, msg, t in bag_data: print(topic) if topic == "/cam_1/aligned_depth_to_color/image_raw/compressed": # # cv_img = bridge.imgmsg_to_cv2(msg, "16UC1") # bridge.encoding_to_dtype_with_channels("16UC1") depth = bridge.compressed_imgmsg_to_cv2(msg) print(depth.shape) # depth = pc2.read_points(msg) # points = np.asanyarray(depth) # 如果bag中保存了该类格式,可用此种方法读取点 cv2.imshow("Image window", depth) cv2.waitKey(3) print("here") elif topic == "cam_1/color/image_raw/compressed": cv_img = bridge.compressed_imgmsg_to_cv2(msg) colorful=cv_img.reshape(-1,3) # pc.map_to(cv_img) # points = pc.calculate(depth) # vtx = np.asanyarray(points.get_vertices()) visual_pcl(visual, depth, colorful) cv2.imshow("Cam1", cv_img) cv2.waitKey(3) print("end")

示例图片:

网友留言(0条)

发表评论