UbiquityRobotics / fiducials

Simultaneous localization and mapping using fiducial markers.
BSD 3-Clause "New" or "Revised" License
263 stars 137 forks source link

fiducial_slam parameter covariance_diagonal doesn't seem to work properly #220

Closed JanezCim closed 4 years ago

JanezCim commented 4 years ago

ROS: Kinetic OS: Ubuntu 16.04

In a launch file I am launching the fiducial_slam with:

  <include file="$(find fiducial_slam)/launch/fiducial_slam.launch">
    <arg name="camera" value="/camera/rgb"/>
    <arg name="map_frame" value="map"/>
    <arg name="odom_frame" value="odom"/>
    <arg name="base_frame" value="base_footprint"/>
    <arg name="publish_tf" value="true"/>
    <arg name="tf_publish_interval" value="0.2"/>
    <arg name="future_date_transforms" value="0.0"/>
    <arg name="publish_6dof_pose" value="false"/>
    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
    <arg name="systematic_error" value="0.01"/>
    <arg name="do_pose_estimation" value="false"/>
    <arg name="covariance_diagonal" value="[0.1, 0.1, 0.1, 0.1, 0.1, 0.1]"/> <!--this is not zero-->
    <!-- <arg name="read_only_map" default="true"/> -->
  </include>

Notice that covariance_diagonal parameter is changed from all 0.0 to all 0.1.

When i launch fiducial_slam and direct my camera toward a marker and listen on a topic /fiducial_pose, I see this:

---
header: 
  seq: 55
  stamp: 
    secs: 1016
    nsecs:   5000000
  frame_id: "map"
pose: 
  pose: 
    position: 
      x: 0.0354128042426
      y: -0.00798936909727
      z: -0.0378735640527
    orientation: 
      x: 0.0061929232813
      y: -0.0115493067347
      z: 0.367441595864
      w: 0.929954264921
  covariance: [0.0037906114657130893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0037906114657130893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0037906114657130893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0037906114657130893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0037906114657130893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0037906114657130893]
---

Notice that covariance diagonal in the topic is always around 0.003 instead of 0.1 like it was set with the parameter covariance_diagonal. Am I understanding the purpose of this parameter wrongly, or is it a bug?

rohbotics commented 4 years ago

@JanezCim Looks like you found a bug.

Not sure why it happens, but its a bug.

rohbotics commented 4 years ago

Ok it looks like the parameter is being interpreted by roslaunch as a string and not an array, so when the node queries for an array, it gets an error. I think this can be solved by adding subst_value="True" to the param line in fiducial_slam.