ROS2(humble + 22.04) depth camera

전체 흐름

1. 뎁스 이미지 수신: ROS 2에서 뎁스 카메라의 depth topic 데이터를 수신.
2. 2D 픽셀 → 3D 좌표 변환: 카메라 매트릭스(내부 파라미터)를 사용하여 픽셀 좌표와 뎁스 값을 3D 좌표로 변환.
3. 3D 그래프 시각화: 변환된 3D 좌표를 Matplotlib, Open3D 또는 다른 라이브러리로 시각화.

---

카메라 파라미터

카메라 매트릭스를 통해 2D 픽셀 좌표를 3D 공간 좌표로 변환합니다. 이때 중요한 파라미터는:

• fx, fy: 카메라의 초점 거리 (focal length, x/y 방향).
• cx, cy: 이미지의 중심 좌표.

이 값은 카메라의 intrinsic 파라미터에서 가져올 수 있습니다. (RealSense SDK 및 ROS 토픽에서 제공).

---

코드 구현

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class DepthTo3DGraph(Node):
    def __init__(self):
        super().__init__('depth_to_3d_graph')

        # Depth 이미지 구독
        self.subscription = self.create_subscription(
            Image,
            '/intel_realsense_r200_depth/depth/image_raw',  # 뎁스 이미지 토픽
            self.depth_callback,
            10
        )
        self.bridge = CvBridge()
        self.fx = 525.0  # 카메라의 focal length (x)
        self.fy = 525.0  # 카메라의 focal length (y)
        self.cx = 319.5  # 이미지 중심 (x)
        self.cy = 239.5  # 이미지 중심 (y)
        self.get_logger().info("Depth to 3D Graph Node started.")
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111, projection='3d')

    def depth_callback(self, msg):
        try:
            # ROS Image를 OpenCV 뎁스 이미지로 변환
            depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
            depth_image = np.array(depth_image, dtype=np.float32)

            # 이미지의 모든 픽셀을 3D 좌표로 변환
            height, width = depth_image.shape
            x_coords, y_coords = np.meshgrid(np.arange(width), np.arange(height))

            # 3D 좌표 계산
            z = depth_image / 1000.0  # 뎁스 값 (단위: m, RealSense는 mm 단위)
            x = (x_coords - self.cx) * z / self.fx
            y = (y_coords - self.cy) * z / self.fy

            # 유효한 값만 필터링 (z > 0)
            mask = z > 0
            x = x[mask]
            y = y[mask]
            z = z[mask]

            # 3D 그래프 그리기
            self.ax.clear()
            self.ax.scatter(x, y, z, c=z, cmap='jet', s=1)  # 3D 점들 시각화
            self.ax.set_xlabel('X (m)')
            self.ax.set_ylabel('Y (m)')
            self.ax.set_zlabel('Z (m)')
            self.ax.set_title("3D Point Cloud from Depth Camera")

            plt.draw()
            plt.pause(0.01)  # 실시간 업데이트
        except Exception as e:
            self.get_logger().error(f"Error processing depth image: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = DepthTo3DGraph()
    try:
        plt.ion()  # Matplotlib 인터랙티브 모드
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()
    plt.ioff()
    plt.show()

if __name__ == '__main__':
    main()

 

코드 설명

1. 카메라 파라미터:
   • fx, fy: 초점 거리. (RealSense R200 기본값: 525.0)
   • cx, cy: 이미지 중심 좌표.
2. 2D → 3D 변환:
   수식:x=(u−cx)⋅zfx,y=(v−cy)⋅zfy,z=depth valuex = \frac{(u - c_x) \cdot z}{f_x}, \quad y = \frac{(v - c_y) \cdot z}{f_y}, \quad z = \text{depth\ value}x=fx​(u−cx​)⋅z​,y=fy​(v−cy​)⋅z​,z=depth value
   • u, v: 이미지 픽셀 좌표.
   • z: 뎁스 값.
3. 3D 점 시각화:
   • Matplotlib의 scatter를 사용해 점을 3D 그래프에 표시.
   • **c=z**를 사용해 뎁스 값에 따라 색상을 다르게 표시.
4. 실시간 업데이트:
   • plt.draw()와 plt.pause(0.01)를 사용해 실시간으로 그래프를 업데이트.

결과

• 뎁스 이미지의 각 픽셀이 3D 좌표로 변환되어 3D 점 그래프로 나타난다.
• 뎁스 값에 따라 점의 색상이 변경.

---

확장 가능성

1. PointCloud 메시지로 변환:
   ROS 2의 sensor_msgs/PointCloud2로 퍼블리시하여 다른 노드에서 사용.
2. Open3D를 사용해 실시간 3D 시각화 성능 개선.
3. 3D 점 클라우드 저장: PCD 또는 XYZ 포맷으로 저장하여 분석할 수 있음.