이미지 세그멘테이션 결과를 이용해서 포인트 클라우드 복원하고 색칠하기 (Point cloud colorization using segmentation result)

사담 : 크게 어렵지 않을 태스크라고 생각했는데, 삽질을 많이 해서 의외로 시간이 많이 들었던 일... 머리가 약간 빠질 뻔 했으나 다행히 해결해 머리카락을 사수할 수 있었습니다. 혹여나 저같이 해답을 찾아 빙빙~ 도는 분이 계실까 하여 공유합니다. 애증의 리얼센스

 

본론만 보실 분은 "접근 2. depth_image_proc 사용하기"로 가기~ 

 

Task : 3D reconstruction을 진행할 때, segmentation result를 texture로 사용하기

 

접근 1 : ros2 realsense2_camera 패키지 이용

개발 환경 : Ubuntu 22.04 ROS2 humble
사용 장비 : Intel Realsense D435
언어 : C++

 

https://github.com/IntelRealSense/realsense-ros/tree/ros2-legacy 이 Realsense ros2 패키지를 빌드한 후

ros2 launch realsense2_camera rs_pointcloud_launch.py 를 실행하면

 

이런 st로 편하게~ 복원을 해 주는 런치파일이 존재합니다.

리얼센스같은 Depth Camera로 3D 복원을 하는 단계를 간단히 살펴보면, 

 

1.  하드웨어에서 Depth, RGB 이미지 프레임을 가져온다. 
2. extrinsic parameter를 사용해서 RGB 이미지 좌표계를 Depth 이미지 좌표계에 Align 시킨다. 
3. Depth 이미지 차원만큼 Point cloud를 선언하고 포인트 클라우드를 복원한다.  (Depth 카메라 intrinsic parameter 사용) 
: 자세한 내용은 (https://medium.com/yodayoda/from-depth-map-to-point-cloud-7473721d3f) 이 글을 참고해주세요. 
4. RGB 프레임에서 컬러값을 가져와서 두 프레임 크기가 다를 경우 Depth 이미지 크기만큼 resize 해 준 후 RGB 값을 이용해서 point cloud 컬러값을 지정한다. 

 

이와 같은 단계로 이루어집니다. 

 

ros2 realsense2_camera의 realsense-ros/realsenes2_camera/examples/pointcloud/rs_pointcloud_launch.py를 살펴보면,

이 파일은  realsense-ros/realsenes2_camera/launch/rs_launch.py을 호출하고 

{'name': 'pointcloud.stream_filter',     'default': '2', 'description': 'texture stream for pointcloud'},

 

여기서 stream_filter 를 지정하는 것을 볼 수 있습니다. 

 

포인트 클라우드를 실제로 발행하는 부분은 realsense-ros/realsenes2_camera/src/base_realsense_node.cpp 의 

void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset)
{
    std::string frame_id = OPTICAL_FRAME_ID(DEPTH);
    _pc_filter->Publish(pc, t, frameset, frame_id);
}

...
rs2_stream texture_source_id = static_cast<rs2_stream>(_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER));
    bool use_texture = texture_source_id != RS2_STREAM_ANY;
    static int warn_count(0);
    static const int DISPLAY_WARN_NUMBER(5);
    rs2::frameset::iterator texture_frame_itr = frameset.end();
    if (use_texture)
    {
        std::set<rs2_format> available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 };

        texture_frame_itr = std::find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f)
                                {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) &&
                                            (available_formats.find(f.get_profile().format()) != available_formats.end()); });
        if (texture_frame_itr == frameset.end())
        {
            warn_count++;
            std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast<float>(texture_source_id));
            ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER, "No stream match for pointcloud chosen texture " << texture_source_name);
            return;
        }
        warn_count = 0;
    }

 

여기서 point cloud의 texture 소스를 어디서 가져올 지 지정하도록 되어 있습니다. 

 

이 RS2_STREAM...은 librealsenes에서 지정하는 자료형으로서 /usr/local/include/librealsense2/h/rs_sensor.h파일에서 

 

typedef enum rs2_stream
{
    RS2_STREAM_ANY,
    RS2_STREAM_DEPTH                            , /**< Native stream of depth data produced by RealSense device */
    RS2_STREAM_COLOR                            , /**< Native stream of color data captured by RealSense device */
    RS2_STREAM_INFRARED                         , /**< Native stream of infrared data captured by RealSense device */
    RS2_STREAM_FISHEYE                          , /**< Native stream of fish-eye (wide) data captured from the dedicate motion camera */
    RS2_STREAM_GYRO                             , /**< Native stream of gyroscope motion data produced by RealSense device */
    RS2_STREAM_ACCEL                            , /**< Native stream of accelerometer motion data produced by RealSense device */
    RS2_STREAM_GPIO                             , /**< Signals from external device connected through GPIO */
    RS2_STREAM_POSE                             , /**< 6 Degrees of Freedom pose data, calculated by RealSense device */
    RS2_STREAM_CONFIDENCE                       , /**< 4 bit per-pixel depth confidence level */
    RS2_STREAM_MOTION                           , /**< Native stream of combined motion data (incl. accel & gyro) */
    RS2_STREAM_COUNT
} rs2_stream;
const char* rs2_stream_to_string(rs2_stream stream);

 

이와 같이 지정하고 있습니다. 그래서 stream_filter를 지정하면 스트리밍되고 있는 데이터 중에서 타입을 저기서 가지고 와서 texture를 입히게 되는 것입니다. 

 

그래서 처음에는 세그멘테이션 결과값을 내 쪽에서 RS2_STREAM 형태로 스트리밍을 따로 해주면 launch 파일만 고치면 되겠구나~ 라고 생각했습니다. 근데 리얼센스 ros2 패키지가 추상화가 되게 잘 되어 있어서 하드웨어에서 이미지를 긁어와서 발행하는 부분도 되게 숨겨져 있고... RS2_STREAM 형태로 스트리밍을 시작하는 부분도 못 찾겠고.... 아무튼 좀 애로가 있었습니다. 

 

근데 이 리컨 과정을 일일이 제가 작성하기에는 또 일이 너무 많지 않냐고요 ㅠ.ㅠ extrinsic 가져와서 다 맞춰주는 것도 번거롭꼬,.,

 

접근 2 : depth_image_proc 패키지 사용하기

 

모든 게 다 있는 경이로운 ROS의 패키지 제공을 이용해보도록 하겠습니다. 

https://github.com/ros-perception/image_pipeline

GitHub - ros-perception/image_pipeline: An image processing pipeline for ROS.

 

이 엄청난 패키지는 1. 카메라 파라미터, 2. rectify 된 컬러이미지, 3. rectify 된 depth 이미지 를 구독해서 point cloud 로 발행해줍니다. 

미쳐따

 

ROS2를 사용할 경우 그냥 클론해서 colcon build 해 주면 되긴 하는데, 브랜치를 자기 Distro에 맞게 설정해줘야 합니다. 안 그러면 yaml::cpp 가 link 가 안 된다느니 하는 에러가 뜨더라구요. realsense2_ros 패키지를 사용하기 때문에 만약 local로 해당 패키지를 빌드해놓았다면 같은 폴더에 넣어서 빌드해주기~

 

그리고 이 패키지를 사용할 때 주의할 점! 

이 패키지가 여러 토픽을 구독하다 보니 당연히 message_filter를 사용합니다. message_filter는 토픽들의 헤더의 timestamp가 일정 기준을 충족해야 콜백함수에 들어가면서 전체 프로세스가 진행되는데, 

만약 rosbag 파일을 사용하고 timestamp가 안 맞는 경우 point cloud가 발행되지 않겠죠 ?

 

그리고 각각의 frame_id tf 관계도 주의하기

 

depth_image_proc에서 구독하는 토픽을 segmentation result 토픽으로 설정해 준 결과는 이와 같습니다. 

 

https://youtu.be/SIvJJxGIl_0?si=J8T1x-AJYnSjjXl3

 

세그멘테이션 결과값을 texture로 이용해서 리얼센스에서 point cloud를 발행하도록 할 수 있습니다.