LIDAR and Stereo Camera Calibration. Remap input topics

asked 2019-10-25 01:46:39 -0500

Astronaut gravatar image

updated 2019-10-25 01:53:39 -0500

Hi All

I have Quanergy LIDAR and PointGrey Grasshoppers RGB cameras that are connected as Master Slave and work as stereo Camera. I want to do Sensor Fusion of LIDAR and Cemeras and for that need to calibrate the LIDAR and Cameras. I have python code but is not working. I took the code from link text and modified to work for my sensors. So This package assumes that the bag file has at least the following topic names and message types by default, these can be modified in the launch scripts.

/sensors/camera/camera_info (sensor_msgs/CameraInfo)
/sensors/camera/image_color (sensor_msgs/Image)
/sensors/velodyne_points    (sensor_msgs/PointCloud2)

In my case these are


Here are the scripts

The main ROS node which handles the topics
    camera_info - [str] - ROS sensor camera info topic
    image_color - [str] - ROS sensor image topic
    velodyne - [str] - ROS velodyne PCL2 topic
    camera_lidar - [str] - ROS projected points image topic

Outputs: None
def listener(camera_info, image_raw, points, camera_lidar=None):
    # Start node
    rospy.init_node('calibrate_camera_lidar', anonymous=True)
    rospy.loginfo('Current PID: [%d]' % os.getpid())
    rospy.loginfo('Projection mode: %s' % PROJECT_MODE)
    rospy.loginfo('CameraInfo topic: %s' % camera_info)
    rospy.loginfo('Image topic: %s' % image_raw)
    rospy.loginfo('PointCloud2 topic: %s' % points)
    rospy.loginfo('Output topic: %s' % camera_lidar)

    # Subscribe to topics
    info_sub = message_filters.Subscriber(camera_info, CameraInfo)
    image_sub = message_filters.Subscriber(image_raw, Image)
    Sensor_sub = message_filters.Subscriber(points, PointCloud2)

    # Publish output topic
    image_pub = None
    if camera_lidar: image_pub = rospy.Publisher(camera_lidar, Image, queue_size=5)

    # Synchronize the topics by time
    ats = message_filters.ApproximateTimeSynchronizer(
        [image_sub, info_sub, Sensor_sub], queue_size=5, slop=0.1)
    ats.registerCallback(callback, image_pub)

    # Keep python from exiting until this node is stopped
    except rospy.ROSInterruptException:
        rospy.loginfo('Shutting down')

if __name__ == '__main__':

    # Calibration mode, rosrun
    if sys.argv[1] == '--calibrate':
        camera_info = '/stereo/left/camera_info'
        image_raw = '/stereo/left/image_raw'
        points = '/Sensor/points'
        camera_lidar = None
        PROJECT_MODE = False
    # Projection mode, run from launch file
        camera_info = rospy.get_param('camera_info_topic')
        image_raw = rospy.get_param('image_color_topic')
        points = rospy.get_param('points_topic')
        camera_lidar = rospy.get_param('camera_lidar_topic')
        PROJECT_MODE = bool(rospy.get_param('project_mode'))

    # Start keyboard handler thread
    if not PROJECT_MODE: start_keyboard_handler()

    # Start subscriber
    listener(camera_info, image_raw, points, camera_lidar)

and display lidar camera calibration

   ?xml version="1.0" encoding="UTF-8"?>
    <arg name="camera" default="/stereo/left" />
    <!-- Play rosbag record -->
    <include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
        <arg name="bagfile" value="2019-10-24-16-56-30.bag" />
       <!-- Nodelet manager for this pipeline -->
        output="screen" />

        name="image_proc_node1" />

    <!-- Run image_proc/rectify nodelet -->
        args="load image_proc/rectify lidar_camera_manager --no-bond" >

        <!-- Remap input topics -->
        <remap from="image_mono"  to="$(arg camera)/image_rect_color" />
        <remap from="camera_info" to="$(arg camera)/camera_info" />

        <!-- Remap output topics -->
        <remap from="image_rect" to="$(arg camera)/image_rect" />

    <!-- Wire static transform from the world to velodyne frame -->
        args="-0.0 -0 ...
edit retag flag offensive close merge delete


Can you tell what the exact problem is that you are having?

Choco93 gravatar image Choco93  ( 2019-10-25 07:53:58 -0500 )edit

when try to perform calibration using the matplotlib GUI to pick correspondences in the camera and the LiDAR frames wirh roslaunch lidar_camera_calibration play_rosbag.launch bagfile:=/path/to/file.bag and rosrun lidar_camera_calibration --calibrate it doesnt open the GUI but it compile. So any help?

Astronaut gravatar image Astronaut  ( 2019-10-26 05:00:34 -0500 )edit