Fusion

cd ~/catkin_ws
catkin_make
source devel/setup.bash

 

라즈베리파이에서 카메라 이미지와 포인트 클라우드 데이터를 실시간으로 퓨전하여 시각화하려면,

ROS를 사용하여 두 데이터를 동기화하고, 이를 하나의 이미지로 변환하여 시각화하는 방법이 필요.

 

이는 ROS 노드를 사용하여 카메라 이미지와 포인트 클라우드 데이터를 동시에 구독하고,

실시간으로 퓨전된 이미지를 생성하여 시각화하는 것을 의미.

 

ROS 환경 설정

먼저, ROS 패키지와 필요한 의존성을 설치합니다.

 

새 ROS 패키지 생성:

bash

 

 

cd ~/catkin_ws/src
catkin_create_pkg camera_lidar_fusion rospy sensor_msgs cv_bridge image_transport

 

 

코드 저장: camera_lidar_fusion/scripts 디렉토리에 camera_lidar_fusion.py라는 파일을 생성하고, 아래의 코드를 저장.

 

빌드 및 소스

권한 부여

cd ~/catkin_ws/src/camera_lidar_fusion/scripts
chmod +x camera_lidar_fusion.py

 

 

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import PointCloud2, Image
import sensor_msgs.point_cloud2 as pc2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import cv2

class CameraLidarFusion:
    def __init__(self):
        rospy.init_node('camera_lidar_fusion', anonymous=True)
        
        # Initialize subscribers and publishers
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.image_callback)
        self.pc_sub = rospy.Subscriber("/camera/depth/points", PointCloud2, self.pc_callback)
        self.fusion_pub = rospy.Publisher("/fusion_image", Image, queue_size=10)
        
        # Initialize variables to store the data
        self.image_data = None
        self.pc_data = None

    def image_callback(self, image_msg):
        try:
            self.image_data = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
        except CvBridgeError as e:
            rospy.logerr(e)

    def pc_callback(self, pc_msg):
        self.pc_data = list(pc2.read_points(pc_msg, field_names=("x", "y", "z"), skip_nans=True))
        self.fuse_data()

    def fuse_data(self):
        if self.image_data is None or self.pc_data is None:
            return
        
        # Create an empty image for fusion result
        fusion_image = self.image_data.copy()

        for point in self.pc_data:
            x, y, z = point
            # Map lidar points to the image plane (requires camera calibration parameters)
            u = int((x / z) * focal_length + cx)
            v = int((y / z) * focal_length + cy)
            
            if 0 <= u < fusion_image.shape[1] and 0 <= v < fusion_image.shape[0]:
                distance = np.sqrt(x*x + y*y + z*z)
                color = self.distance_to_color(distance)
                cv2.circle(fusion_image, (u, v), 3, color, -1)
        
        # Convert the fusion image to ROS Image message and publish
        try:
            fusion_msg = self.bridge.cv2_to_imgmsg(fusion_image, "bgr8")
            self.fusion_pub.publish(fusion_msg)
        except CvBridgeError as e:
            rospy.logerr(e)

    def distance_to_color(self, distance):
        # Normalize distance and apply color map
        max_distance = 10.0
        normalized_distance = min(distance / max_distance, 1.0)
        color_value = int(normalized_distance * 255)
        return (color_value, 0, 255 - color_value)

if __name__ == '__main__':
    try:
        fusion_node = CameraLidarFusion()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

YUYV 형식의 경우 변환을 해주어야 한다.

    def image_callback(self, image_msg):
        try:
            # Convert the ROS Image message to a NumPy array
            yuy_image = self.bridge.imgmsg_to_cv2(image_msg, "passthrough")
            # Convert YUYV to BGR
            self.image_data = cv2.cvtColor(yuy_image, cv2.COLOR_YUV2BGR_YUYV)
        except CvBridgeError as e:
            rospy.logerr(e)

 

 

주요 포인트

이미지 및 포인트 클라우드 데이터 구독

카메라 이미지는 /camera/image_raw 토픽에서 구독.

포인트 클라우드는 /camera/depth/points 토픽에서 구독.

 

데이터 동기화 및 퓨전

두 데이터를 동기화하기 위해 각각의 콜백 함수에서 데이터를 저장하고, fuse_data 함수에서 퓨전 작업을 수행.

 

포인트 클라우드 데이터를 이미지 좌표로 매핑

카메라 캘리브레이션 파라미터(focal_length, cx, cy)를 사용하여 포인트 클라우드 데이터를 이미지 좌표로 매핑

매핑된 포인트의 거리를 색상으로 변환하여 이미지에 그려줌.

 

실행 및 시각화:

roslaunch 파일을 사용하여 노드를 실행하고, RViz 또는 다른 이미지 뷰어를 사용하여 퓨전된 이미지를 시각화할 수 있습니다.

 

launch 파일 예제:

launch 디렉토리에 camera_lidar_fusion.launch 파일을 생성합니다.

<launch>
    <node pkg="camera_lidar_fusion" type="camera_lidar_fusion.py" name="camera_lidar_fusion" output="screen"/>
</launch>

 

노드 실행:

roslaunch camera_lidar_fusion camera_lidar_fusion.launch