Realsense depth 활용

Realsense example들이 비교적 잘 짜여져 있다

하지만 항상 Document를 확인해서 할 수 없다

자주 쓰는 친구들을 정리해보기로 했다!

 

 

[Reference]

• [1] Realsense example Document
• [2] Realsense Depth filtering
• [3] Realsense with MaskRCNN depth estimation

---

1. align-depth2color.py 예제를 보면

#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 자료형이 다른데 대충 좀 궁금해서 살펴봤다 참고 요기 문서에 가면 볼 수 있다
• line by line으로 코드를 보다 보면 윙? 이게 어떻게 된일이람? 하는 경우가 많다(나는 그렇다)

그러니깐 전체적으로

• rs.pipeline()으로 realsense를 가져와서
• config 설정파일을 설정하고
• pipeline을 start 하여
• 원하는 센서 값들 scale을 얻고
• depth와 이미지의 align을 맞춰서 반환하고
• get을 통해 맞춰진 값들을 얻고 --> 결과적으로 데이터를 얻을 수 있다!

tip 추가적으로 depth에 color를 입히고 싶으면 applycolormap*

---

2. Depth Filtering을 통한 realsense depth 값 보정하기[추후 정리]

---

3. 함수화 시켜서 data capture! 저장! 다시 꺼내 보기!

#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)