[ROS-python] ZED camera로 depth 추정하기 (stereo-vision)

Concept

stereo vision에서의 depth estimation

stereo vision에서는 두 개의 광축 평행한 카메라에서 촬영된 이미지의 동일한 점 P에 대하여, 삼각 측량(triangulation)을 통해 depth를 추정할 수 있다.

Depth estimation formula by stereo camera

Depth = (Baseline * Focal Length) / Disparity

여기서 Baseline은 두 카메라 렌즈 사이의 거리, Focal Length는 카메라 렌즈의 초점 거리, Disparity는 두 카메라의 이미지 평면 상에서 대응되는 특징점들 간의 거리 차이이다.

일반적으로 카메라 정보(baseline, focal length) 값은 주어져 있으므로, disparity만 제대로 추정하면 될 것이다.

disparity를 계산하기 위해서는 먼저 이미지 정합(Image Rectification)을 통해 두 이미지를 동일한 평면 상에 투영하고, Feature extraction과 Feature matching 알고리즘을 통해 특징점과 그 대응점을 찾아야 한다.

image rectification process는 다음과 같다.

1. Camera Calibration
   : camera intrinsic, extrinsic parameters를 통해 projection matrix 구하기

camera projection matrix
$P=K[R|T]$

$K$ : camera intrinsic parameters
$R$, $t$ : camera extrinsic parameters (rotation, translation)

2. Get Homography matrix
   : projection matrix를 이용하여 homography matrix를 계산한다. homography matrix를 통해 한 이미지 평면을 다른 평면으로 변환할 수 있다 (homography matrix란, 두 카메라 간 기하학적 변환을 나타내는 행렬).

homography matrix
$H = K_r * R_r * K_l^{-1}$

즉, homography 행렬 $H$는 오른쪽 카메라의 intrinsic parameter matrix인 $K_r$, 오른쪽 카메라의 회전 행렬인 $R_r$, 왼쪽 카메라의 intrinsic parameter matrix인 $K_l$의 역행렬을 곱하여 계산된 값이다.

Rectified image coordinates
$(u', v') = H * (u, v, 1)^T$

이 homography matrix를 사용하면, 이미지의 한 점 $(u, v, 1)^T$에서 다른 이미지의 좌표 $(u′, v′)$ 로의 변환을 계산할 수 있게 되는 것이다.

3. Feature extraction and matching
   : SIFT, SURF와 같은 feature extraction algorihtm을 사용하여 두 이미지의 특징점을 검출하고, feature matching을 통해 대응 관계 파악

왼쪽, 오른쪽 특징점을 매칭하고 나면, 이제 정합된 이미지에서 대응되는 특징점 쌍 간의 disparity를 계산한다.

Disparity의 계산 식은 다음과 같다.

Disparity calculation

Disparity = ($x_L - x_R$)

여기에서 $x_L$은 left image의 특징점 x 좌표, $x_R$은 right image의 특징점 좌표이다.

Application

zed camera에서의 depth 값은 topic으로 발행된다. 나는 ZED2 를 사용하였으므로, /zed2/zed_node/depth/depth_registered에서 topic을 읽을 수 있었다.

rviz에서 해당 이미지 토픽을 띄워보면 다음과 같다.

이러한 grayscale depth image에서 픽셀의 밝기값이 해당 픽셀의 depth information이 된다.

특정 영역의 bounding box를 지정하여, 해당 구역의 depth 정보를 추출하는 python code를 적용하여 보자.

import rospy
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np

drawing = False  
ix, iy = -1, -1
bbox = []

def select_bbox(event, x, y, flags, param):
    global ix, iy, drawing, bbox

    if event == cv2.EVENT_LBUTTONDOWN:
        drawing = True
        ix, iy = x, y

    elif event == cv2.EVENT_MOUSEMOVE:
        if drawing == True:
            img_copy = param.copy()
            cv2.rectangle(img_copy, (ix, iy), (x, y), (0, 255, 0), 2)
            cv2.imshow("Color Image", img_copy)

    elif event == cv2.EVENT_LBUTTONUP:
        drawing = False
        bbox = [ix, iy, x, y]
        cv2.rectangle(param, (ix, iy), (x, y), (0, 255, 0), 2)
        cv2.imshow("Color Image", param)

class ZEDCameraViewer:
    def __init__(self):
        self.bridge = CvBridge()
        self.color_sub = rospy.Subscriber("/zed2/zed_node/rgb/image_rect_color", Image, self.color_callback)
        self.depth_image = None
        rospy.Subscriber("/zed2/zed_node/depth/depth_registered", Image, self.depth_callback)
        rospy.loginfo("ZED Camera Viewer has started.")

    def color_callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            cv2.namedWindow("Color Image")
            cv2.setMouseCallback("Color Image", select_bbox, cv_image)

            while True:
                cv2.imshow("Color Image", cv_image)
                k = cv2.waitKey(1) & 0xFF
                if k == 27 or bbox:  
                    break

            if bbox and self.depth_image is not None:
                self.calculate_average_depth()

            cv2.destroyAllWindows()
        except CvBridgeError as e:
            print(e)

    def depth_callback(self, data):
        try:
            self.depth_image = self.bridge.imgmsg_to_cv2(data, "32FC1")
        except CvBridgeError as e:
            print(e)

    def calculate_average_depth(self):
        x_min, y_min, x_max, y_max = bbox
        depth_values = self.depth_image[y_min:y_max, x_min:x_max]
        # print(self.depth_image)
        valid_depths = depth_values[np.isfinite(depth_values)]
        if valid_depths.size > 0:
            average_depth = np.mean(valid_depths)
            rospy.loginfo(f"Average depth in bbox: {average_depth:.3f} meters")
        else:
            rospy.loginfo("No valid depth data in the specified bbox.")

if __name__ == "__main__":
    rospy.init_node("zed_camera_viewer", anonymous=True)
    viewer = ZEDCameraViewer()
    rospy.spin()

위 코드는 카메라 이미지 토픽과 depth 이미지 토픽을 불러와서, cv2의 mouse event를 활용하여 cv window 내 바운딩 박스 영역을 지정하면, 해당 영역의 depth average 값을 구하여 거리를 추정한다.