獲取點云
借到了相機(jī)d435i。開始嘗試獲取點云。
要求：檢測磚垛的第一層，計數(shù)并定位，便于之后機(jī)器人的操作。這里第一層磚塊不是規(guī)則排布的。
目的：這篇文章主要記錄如何與相機(jī)交互，獲得點云，還有想辦法把不需要的點云刪除。
思路：俯拍磚垛。然后在z軸方向設(shè)置閾值，已達(dá)到刪除第一層磚以外的平面的目的。
這里的代碼我參考了
獲取數(shù)據(jù)并轉(zhuǎn)化成點云
https://blog.csdn.net/weixin_43719386/article/details/117066229
找極值設(shè)置閾值
https://blog.csdn.net/a394467238/article/details/128079667?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7EPayColumn-1-128079667-blog-119518855.235%5Ev36%5Epc_relevant_yljh&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7EPayColumn-1-128079667-blog-119518855.235%5Ev36%5Epc_relevant_yljh&utm_relevant_index=1
********************************************************************************************************
導(dǎo)入必要的庫，open3d用于點云的處理和可視化。numpy用于數(shù)組的處理。pyrealsense2用于和realsense相機(jī)交互。
import open3d as o3d
import numpy as np
import pyrealsense2 as rs
建立管道并配置深度和彩色流
#Configure the pipeline to depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
開始流
#Start streaming
pipe_profile = pipeline.start(config)
建立對齊對象，將深度流向彩色流對齊
#Create Alignment
pc = rs.pointcloud()
align_to_color = rs.align(rs.stream.color)
try:
跳過初始的20幀，以便相機(jī)穩(wěn)定下來
? ? #Skip a few frames
? ? for _ in range(20):
? ? ? ? pipeline.wait_for_frames()
獲取對齊后的深度和彩色幀
? ? frames = pipeline.wait_for_frames()
? ? frames = align_to_color.process(frames)
? ? depth_frame = frames.get_depth_frame()
? ? color_frame = frames.get_color_frame()
將深度幀和彩色幀轉(zhuǎn)換為numpy數(shù)組以便后續(xù)處理
? ? # Convert images to numpy arrays
? ? depth_image = np.asanyarray(depth_frame.get_data())
? ? color_image = np.asanyarray(color_frame.get_data())
獲取相機(jī)的內(nèi)參，這里用的是pyrealsense2
? ? # Intrinsics
? ? profile = frames.get_profile()
? ? intrinsics = profile.as_video_stream_profile().get_intrinsics()
這里用的是open3d? ?
# 轉(zhuǎn)換為open3d中的相機(jī)參數(shù)
? ? pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
? ? ? ? intrinsics.width, intrinsics.height, intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy)
從深度圖創(chuàng)建點云，得到初始點云
? ? # Create point cloud from depth image
? ? depth = o3d.geometry.Image(depth_image)
? ? pcd = o3d.geometry.PointCloud.create_from_depth_image(depth, pinhole_camera_intrinsic)
? ? # Visualize initial point cloud
? ? o3d.visualization.draw_geometries([pcd],window_name="original",
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? width=1024, height=768,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? left=50, top=50,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? mesh_show_back_face=False)
? ?刪除第一層以外的平面
? ?從初始點云獲得各個點的坐標(biāo)，找出z的最小值作為第一層的代表
? ?這里因為我使用直立的麻將作為模型，高度大約為3cm，用zmin+h/2作為閾值
? ?保留z值小于閾值的點，即為第一層的點
? ?創(chuàng)建點云對象level_pcd,將轉(zhuǎn)換后的點云對象賦值給level_pcd的points屬性
? ?保存第一層的點云level_pcd為pcd文件，命名為level_1.pcd
? ? # Remove second layer
? ? point_stack = np.asarray(pcd.points)
? ? z = point_stack[:, 2]
? ? zmin = np.min(z)
? ? print('min z', zmin)
? ? threshold = zmin + 0.015
? ? level_point = point_stack[z < threshold]
? ? level_pcd = o3d.geometry.PointCloud()
? ? level_pcd.points = o3d.utility.Vector3dVector(level_point)
? ? # Visualize first layer point cloud
? ? o3d.visualization.draw_geometries([level_pcd], window_name="Level 1",
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? width=1024, height=768,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? left=50, top=50,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? mesh_show_back_face=False)
? ? o3d.io.write_point_cloud("level_1.pcd", level_pcd)
finally:
? ? pipeline.stop()
? ? print('done')
后面要開始濾波，之后再寫吧?
#Statistical outlier removal
? ? print("Statistical outlier removal")
? ? cl, ind = level_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
? ? sor_cloud = level_pcd.select_by_index(ind)
? ? print("before",level_pcd)
? ? print("after",sor_cloud)
? ? # Visualize filtered point cloud
? ? o3d.visualization.draw_geometries([sor_cloud], window_name="Statistical Filtering",
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? width=1024, height=768,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? left=50, top=50,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? mesh_show_back_face=False)
