Tracking QR codes within Gazebo result in off-centre transforms

asked 2018-09-21 09:24:07 -0600

Elisabeth gravatar image

updated 2018-09-23 07:36:19 -0600

lucasw gravatar image

Hi all, I'm trying to run the package Fiducials on a quadcopter within a gazebo simulation. There are two nodes that are ran during this, the aruco_detect node that recognises the QR codes within the camera feed and the fiducial_slam node that calculates the relative position of the robot. The aruco_detect node works fine, with the fiducials being recognised within the virtual camera stream.There is a map file ~/.ros/slam/map.txt I have edited to produce the markers you can see within the RViz simulation that represent the position of the markers relative to the world frame. These markers are used for the pose estimation. The markers are red if they are in view and green is they are declared in the map but not within camera view. However, the fiducial_slam produces slightly offset transforms 'fid100' and fid101 for the the fiducial markers named 100 and 101. I cannot work out the cause of this offset. I think it may be something to do with fid0 frame but I'm not entirely sure what this frame signifiies as it does not say within the documentation. I include the launch file that I launch the two nodes from. No changes from the original source code have been made. I am running ROS kinetic on ubuntu 16.04 and gazebo 7

       <arg name="camera" default="/cam/camera"/>
      <arg name="image" default="image"/>
      <arg name="transport" default="compressed"/>
      <arg name="fiducial_len" default="0.44237"/> 
      <arg name="dictionary" default="7"/>
      <arg name="do_pose_estimation" default="true"/>
     <node pkg="aruco_detect" name="aruco_detect"
    type="aruco_detect"  respawn="false">
    <param name="image_transport" value="$(arg transport)"/>
    <param name="publish_images" value="true" />
    <param name="fiducial_len" value="$(arg fiducial_len)"/>
    <param name="dictionary" value="$(arg dictionary)"/>
    <param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>
    <remap from="/camera/compressed"
        to="$(arg camera)/$(arg image)/$(arg transport)"/>
    <remap from="/camera_info" to="$(arg camera)/camera_info"/></node>

    <arg name="map_frame" default="world"/> 
  <arg name="base_frame" default="base_link"/>
  <arg name="publish_tf" default="false"/>
  <arg name="future_date_transforms" default="0.0"/>
  <arg name="publish_6dof_pose" default="true"/>

  <node type="fiducial_slam" pkg="fiducial_slam" 
    name="fiducial_slam" output="screen">

    <param name="map_file" value="$(env HOME)/.ros/slam/map.txt" />
    <param name="map_frame" value="$(arg map_frame)" />
    <param name="odom_frame" value="" /> 
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="future_date_transforms" value="$(arg future_date_transforms)" />
    <param name="publish_6dof_pose" value="$(arg publish_6dof_pose)" />
    <param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>

    <param name="fiducial_len" value="$(arg fiducial_len)"/>
    <remap from="/camera_info" to="$(arg camera)/camera_info"/>
    <remap from="camera_frame" to="cam_optical_frame"/>


Here are screenshots to make this a lot clearer in a shareable link: Drone on the ground looking at QR codes in gazebo :

image description

Drone on ground looking at QR codes in Rviz ( robot goes through floor)

image description

There is also a static transform publisher from my cam_optical_frame to my base_link as described by:

<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_optical_cam_frame"  args="-0.4 0.0 0 1.57 0 -1.57 /base_link /cam_optical_frame 100"/>


/Camera_info topic shows ... (more)

edit retag flag offensive close merge delete


The error is the offset between the fid104 & fid105 tf frames and the red squares? Are you confident the squares are actually where they need to be to line up with gazebo? It looks like from the Image view that the tf frames are spot on the center of the observed tags.

lucasw gravatar image lucasw  ( 2018-09-21 11:27:06 -0600 )edit

Does your camera have distortion coefficients? I wonder if some parts of your system are using them and others are not, creating the error.

lucasw gravatar image lucasw  ( 2018-09-21 11:27:58 -0600 )edit

Hi, yes the squares are in the correct position. I wondered that so I ran the for monocular cameras found at the opencv package and my camera still does contain distortion parameters and the camera info topic published by the camera is in the edit. Could I set these as zero?

Elisabeth gravatar image Elisabeth  ( 2018-09-23 02:45:55 -0600 )edit

No, only the D: is the distortion, and it already is all zero. If that was a calibration for a real camera (not a gazebo simulated one) there should be something there. Also is there a possibility that gazebo is using a different camera info than the one other parts of the system are using?

lucasw gravatar image lucasw  ( 2018-09-23 07:15:24 -0600 )edit

Look for two different camera_info topics. Is there a set of launch files online that you are you using you can link to to?

lucasw gravatar image lucasw  ( 2018-09-23 07:16:16 -0600 )edit

<remap from="camera_frame" to="cam_optical_frame"/> looks wrong, but may not be causing any problems either.

lucasw gravatar image lucasw  ( 2018-09-23 07:51:02 -0600 )edit