Development
[CV - Sensor] Realsense d435 Depth with python ( 2d pixels to 3d points ) - 픽셀 값을 이용해서 3차원 공간 상의 위치 구하기
capaca
2023. 3. 18. 04:01
2D pixel to 3D point deprojection
- depth 카메라를 이용하는 이유 중 하나인 2d pixel 을 이용해서 3차원 위치를 표현하는 방법이다.
- 원래라면 2D camera를 이용해서 calibration을 통해서 extrinsic parameter를 얻어서 해야하지만 뎁스카메라에서는 카메라로 부터 물체의 거리를 알 수 있으니 그것을 이용하여 쉽게 3차원 좌표관계를 얻을 수 있다. 아래와 같은 과정을 통해서 결과를 얻을 수 있다.
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(color_frame.profile)
# 2d pixel
depth_pixel = [100, 100]
depth= depth_frame.get_distance(100,100) # depth 값을 얻는 과정
# 그에 대응 하는 3d points 값
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth)
# ()괄호 내의 값들을 순서대로 설명하자면
# depth cam의 intrinsic parameter , depth_pixel(원하는 2d pixel값), 실제 depth 길이 (카메라로 얻을 수 있는)
align-depth2color
#align-depth2color.py
#
import pyrealsense2 as rs
import numpy as np
import cv2
pipeline = rs.pipeline() # realsense pipeline open
config = rs.config() # config class 생성!
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) # stream 종류, size, format 설정등록
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config) # pipeline start
depth_sensor = profile.get_device().first_depth_sensor() # depth sensor에 대한 것들을 얻자
depth_scale = depth_sensor.get_depth_scale() # 깊이 센서의 깊이 스케일 얻음
print("Depth Scale is: ", depth_scale)
clipping_distance_in_meters = 1 # 1 meter, 클리핑할 영역을 1m로 설정
clipping_distance = clipping_distance_in_meters / depth_scale #스케일에 따른 클리핑 거리
align_to = rs.stream.color #depth 이미지를 맞추기 위한 이미지, 컬러 이미지
align = rs.align(align_to) #depth 이미지와 맞추기 위해 align 생성
try:
while True:
frames = pipeline.wait_for_frames() #color와 depth의 프레임셋을 기다림
#frames.get_depth_frame() 은 640x360 depth 이미지이다.
aligned_frames= align.process(frames) #모든(depth 포함) 프레임을 컬러 프레임에 맞추어 반환
aligned_depth_frame = aligned_frames.get_depth_frame() # aligned depth 프레임은 640x480 의 depth 이미지이다
color_frame = aligned_frames.get_color_frame() #컬러 프레임을 얻음
if not aligned_depth_frame or not color_frame: #프레임이 없으면, 건너 뜀
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data()) #depth이미지를 배열로,
color_image = np.asanyarray(color_frame.get_data()) #color 이미지를 배열로
#백그라운드 제거
grey_color = 153
depth_image_3d = np.dstack((depth_image, depth_image, depth_image)) #depth image는 1채널, 컬러 이미지는 3채널
bg_removed = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), grey_color, color_image)
# 클리핑 거리를 깊이 _이미지가 넘어서거나, 0보다 적으면, 회색으로 아니면 컬러 이미지로 반환
#이미지 렌더링
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# applyColorMap(src, 필터) 필터를 적용함 , COLORMAP_JET= 연속적인 색상, blue -> red
# convertScaleAbs: 인자적용 후 절대값, 8비트 반환
images = np.hstack((bg_removed, depth_colormap)) #두 이미지를 수평으로 연결
cv2.namedWindow('Align Example', cv2.WINDOW_AUTOSIZE) #이미지 윈도우 정의
cv2.imshow('Align Example', images) #이미지를 넣어 윈도우에 보임
key = cv2.waitKey(1) #키 입력
if key & 0xFF == ord('q') or key == 27: #나가기
cv2.destroyAllWindows() #윈도우 제거
break
finally:
pipeline.stop() #리얼센스 데이터 스트리밍 중지
- realsense config.enable_stream 뒤에 depth랑 img 자료형이 다름, 참고 에 가면 볼 수 있다
요약
- rs.pipeline()으로 realsense를 가져와서
- config 설정파일을 설정하고
- pipeline을 start 하여
- 원하는 센서 값들 scale을 얻고
- depth와 이미지의 align을 맞춰서 반환하고
- get을 통해 맞춰진 값들을 얻고 --> 결과적으로 데이터를 얻을 수 있다.
tip 추가적으로 depth에 color를 입히고 싶으면 applycolormap*
함수화
#https://pysource.com
import pyrealsense2 as rs
import numpy as np
import cv2
import matplotlib.pyplot as plt
class RealsenseCamera:
def __init__(self):
# Configure depth and color streams
print("Loading Intel Realsense Camera")
self.pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
# Start streaming
self.pipeline.start(config)
align_to = rs.stream.color
self.align = rs.align(align_to)
def get_frame_stream(self):
# Wait for a coherent pair of frames: depth and color
frames = self.pipeline.wait_for_frames()
aligned_frames = self.align.process(frames)
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
if not depth_frame or not color_frame:
# If there is no frame, probably camera not connected, return False
print("Error, impossible to get the frame, make sure that the Intel Realsense camera is correctly connected")
return False, None, None
# Apply filter to fill the Holes in the depth image
spatial = rs.spatial_filter()
spatial.set_option(rs.option.holes_fill, 3)
filtered_depth = spatial.process(depth_frame)
hole_filling = rs.hole_filling_filter()
filled_depth = hole_filling.process(filtered_depth)
# Create colormap to show the depth of the Objects
colorizer = rs.colorizer()
depth_colormap = np.asanyarray(colorizer.colorize(filled_depth).get_data())
# Convert images to numpy arrays
# distance = depth_frame.get_distance(int(50),int(50))
# print("distance", distance)
depth_image = np.asanyarray(filled_depth.get_data())
color_image = np.asanyarray(color_frame.get_data())
depth_colormap_dim = depth_colormap.shape
color_colormap_dim = color_image.shape
# If depth and color resolutions are different, resize color image to match depth image for display
if depth_colormap_dim != color_colormap_dim:
resized_color_image = cv2.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA)
images = np.hstack((resized_color_image, depth_colormap))
else:
images = np.hstack((color_image, depth_colormap))
# Show images
# cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
# cv2.imshow('RealSense', images)
# cv2.waitKey(1)
# cv2.destroyAllWindows()
return True, color_image, depth_image, depth_colormap
def release(self):
self.pipeline.stop()
#print(depth_image)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
#depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.10), 2)
# Stack both images horizontally
#images = np.hstack((color_image, depth_colormap))
def image_capture(img_path, depth_path):
real = RealsenseCamera()
while True:
ret, bgr_frame, depth_frame, depth_colormap = real.get_frame_stream()
cv2.imshow("depth frame", depth_frame)
cv2.imshow("Bgr frame", bgr_frame)
key = cv2.waitKey(1)
if key == 27:
break
elif key == ord('s'): # wait for 's' key to save and exit
cv2.imwrite(img_path, bgr_frame)
np.save(depth_path , depth_frame)
cv2.destroyAllWindows()
break
real.release()
cv2.destroyAllWindows()
def plot(img, depth, cx, cy):
IMG = cv2.imread(img, cv2.IMREAD_COLOR)
DEPTH = np.load(depth)
cv2.imshow("depth frame", DEPTH)
cv2.imshow("Bgr frame", IMG)
key = cv2.waitKey(1)
if key == 27:
break
cv2.destroyAllWindows()
print(DEPTH[cy,cx])
if __name__=="__main__":
img_path = './data/img.png'
depth_path = './data/depth.npy'
# image_capture(img_path, depth_path)
plot(img_path, depth_path, 672, 313)[Reference]