Camera_Lidar Fusion_3

1. Camera calibration 업데이트 확인

 

이제 이전에 작업했던 calibration 을 적용하여 새로운 bag 파일을 생성한다.

 

rosrun lidar_camera_calibration update_camera_info.py <original_file.bag> <calibration_file.yaml>

 

명령어는 다음과 같은데 이때 update_camera_info.py 에서 수정할 부분이 몇가지 있다.

 

#!/usr/bin/env python3.8 # 1. 여긴 3.8 로 지정해줘야 한다.
# -*- coding: utf-8 -*-

'''
Author  : Heethesh Vhavle
Email   : heethesh@cmu.edu
Version : 1.0.1
Date    : Jan 18, 2019

Description:
Script to update the camera calibration data into the ROSBAG file
Ensure that this file has executable permissions

Example Usage:
$ rosrun lidar_camera_calibration update_camera_info.py rosbag.bag calibration.yaml

Notes:
Make sure this file has executable permissions:
$ chmod +x update_camera_info.py
'''

# Python 2/3 compatibility
from __future__ import print_function

# Built-in modules
import os
import sys
import yaml

# ROS modules
PKG = 'lidar_camera_calibration'
import roslib; roslib.load_manifest(PKG)
import rosbag
import rospy


def load_calibration_data(filename):
    # Open calibration file
    with open(filename, 'r') as stream:
        try:
            #calibration = yaml.load(stream)
            calibration = yaml.load(stream, Loader=yaml.FullLoader) # 2. 뒤에 Loder 부분 추가.

        except yaml.YAMLError as exc:
            rospy.logerr(exc)
            sys.exit(1)

    return calibration


if __name__ == '__main__':

    # Get parameters when starting node from a launch file.
    if len(sys.argv) < 1:
        BAG_FILE = rospy.get_param('filename')
        CALIB_FILE = rospy.get_param('calib_data')
        CAMERA_INFO = rospy.get_param('camera_info')

    # Get parameters as arguments
    else:
        BAG_FILE = sys.argv[1]
        CALIB_FILE = sys.argv[2]
        CAMERA_INFO = '/usb_cam/camera_info'   # 3. 여긴 본인 camera_info 형식에 맞게 수정

    # Load ROSBAG file
    rospy.loginfo('Bag Filename: %s', BAG_FILE)
    bag = rosbag.Bag(BAG_FILE, 'r')

    # Output file
    folder = os.path.dirname(BAG_FILE)
    output_name = os.path.splitext(os.path.basename(BAG_FILE))[0] + '_updated.bag'
    OUTPUT_FILE = os.path.join(folder, output_name)
    os.mknod(OUTPUT_FILE)
    output = rosbag.Bag(OUTPUT_FILE, 'w')

    # Load calibration data
    calibration = load_calibration_data(CALIB_FILE)

    # Update calibration data
    rospy.loginfo('Updating %s data...' % CAMERA_INFO)
    for topic, msg, t in bag.read_messages():
        if topic == CAMERA_INFO:
            msg.D = calibration['distortion_coefficients']['data']
            msg.K = calibration['camera_matrix']['data']
            msg.R = calibration['rectification_matrix']['data']
            msg.P = calibration['projection_matrix']['data']
        output.write(topic, msg, msg.header.stamp if msg._has_header else t)
    rospy.loginfo('Done')

    # Close bag file
    bag.close()
    output.close()

 

일단 3가지 수정사항이 있다.

 

먼저 맨 위에를 python3.8 로 수정하였다. 본인 python 환경에 따라 수정해주면 된다.

 

다음으로 calibration = yaml.load(stream, Loader=yaml.FullLoader) 뒤에 Loader 부분을 추가해줬다. 이를 지정해주지 않으면

E.YAMLLoadWarning 오류가 뜰것이다. 안전한 로더를 명시적으로 지정해줘야 한다.

 

마지막으로 CAMERA_INFO = '/usb_cam/camera_info' 로 usb_cam 으로 수정해주었다. 이 또한 본인의 camera_info 에 맞게 수정해주면 된다.

 

 

이제 다시

rosrun lidar_camera_calibration update_camera_info.py /본인카메라 bag파일 경로/카메라.bag /본인 calibration한 yaml파일경로/ost.yaml

 

경로랑 파일명은 본인걸로 수정해주면,

 

이렇게 update 파일이 추가된다.

 

 

 

 

2. calibration bag 파일 확인

 

이제 display_camera_calibration.launch 를 수정할 차례이다.

 

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <arg name="camera" default="/usb_cam" /> <!--본인 카메라 topic -->

    <!-- Play rosbag record -->
    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
        <arg name="bagfile" value="camera_data_updated.bag" />
    </include>

    <!-- Nodelet manager for this pipeline -->
    <node
        pkg="nodelet"
        type="nodelet"
        args="manager"
        name="lidar_camera_manager"
        output="screen" />

    <node
        pkg="image_proc"
        type="image_proc" 
        name="image_proc_node1" />

    <!-- Run image_proc/rectify nodelet -->
    <node
        pkg="nodelet"
        type="nodelet"
        name="rectify_color"
        args="load image_proc/rectify lidar_camera_manager --no-bond" >
        
        <!-- Remap input topics -->
        <remap from="image_mono" to="$(arg camera)/image_raw" /> <!--본인 카메라 topic image_raw-->
        <remap from="camera_info" to="$(arg camera)/camera_info" /> <!--본인 카메라 topic camera_info-->

        <!-- Remap output topics -->
        <remap from="image_rect" to="$(arg camera)/image_raw" /> <!--본인 카메라 topic image_raw-->
    </node>

    <!-- Run image view to display the unrectified image -->
    <node 
        name="unrectified" 
        pkg="image_view"
        type="image_view"
        respawn="false"
        output="screen">
        
        <!-- Remap input topics -->
        <remap from="image" to="$(arg camera)/image_raw" /> <!--본인 카메라 topic image_raw-->
    </node>

    <!-- Run image view to display the rectified image -->
    <node 
        name="rectified" 
        pkg="image_view"
        type="image_view"
        respawn="false"
        output="screen">
        
        <!-- Remap input topics -->
        <remap from="image" to="$(arg camera)/image_raw" /> <!--본인 카메라 topic image_raw-->
    </node>

</launch>

 

위에 주석처리한 부분을 본인 카메라 토픽으로 수정하고 다음과 같은 명령어를 실행하면

 

roslaunch lidar_camera_calibration display_camera_calibration.launch

 

이렇게 오른쪽처럼 체커보드가 펴진 모습을 확인할 수 있다.