Issues with map building

[mhkabir]

I'm bringing up the package on a stereo camera setup, and the mapping seems to be OK, with the Octomap updating just fine.

The terminal though, is filled with these 2 messages. Can someone tell me what is causing this, and how I can fix it? I'm running ROS Jade.

[ WARN] [1455351424.149550020]: Using latest TF transform instead of timestamp match.

and this one here is almost continuous :

WARNING: coordinates ( (0.398782 0.310725 -0.175888) -> (inf -nan -nan)) out of bounds in computeRayKeys

Thanks!

[birchera]

The first of the warnings you get is most probably due to limited computation power. The tf that you want to look up has a timestamp, e.g. corresponding to the time the stereo image was taken that was used to compute the point cloud. The queue for tf lookup in the past is limited and possibly the queried time is not available any more. The warning informs you that this happened and instead the current time is taken for the tf query.

The second warning can come from a lot of sources. It basically means that you try to perform a collision check in the occupancy map and one of the end-points is nan. The incoming point clouds are pruned for all nan values, therefore I suppose it could come from the navigation or visibility check. Can you share the parameter set that you are running?

[mhkabir]

It seems kind of unlikely that the TF lookup warnings were due to limited computation. In any case, I'll have a look to see if I can increase CPU overhead.

Here is the launchfile :

<arg name="param_file" default="$(find mav_startup)/parameters/area_exploration.yaml" />

    <arg name="tf_frame" default="world" />
    <arg name="resolution" default="0.05" />
    <arg name="visualize_max_z" default="999" />
    <arg name="sensor_max_range" default="5.0" />

    <node name="nbvPlanner" pkg="nbvplanner" type="nbvPlanner" output="screen">

      <rosparam command="load" file="$(arg param_file)" />

      <param name="tf_frame" type="string" value="$(arg tf_frame)" />
      <param name="resolution" type="double" value="$(arg resolution)" />
      <param name="visualize_max_z" type="double" value="$(arg visualize_max_z)" />
      <param name="sensor_max_range" type="double" value="$(arg sensor_max_range)" />

      <param name="map_publish_frequency" type="double" value="2.0" />
      <param name="filter_speckles" type="bool" value="true" />

      <remap from="cam0/camera_info" to="/stereo/cam0/camera_info"/>
      <remap from="cam1/camera_info" to="/stereo/cam1/camera_info"/>

      <remap from="odometry" to="/mav2/odometry"/>

      <remap from="pointcloud" to="/stereo/point_cloud"/>
      <remap from="disparity" to="/stereo/disparity"/>

    </node>

and the area_exploration.yaml :

system/v_max: 0.2
system/dyaw_max: 0.75
system/camera/pitch: [15.0]
system/camera/horizontal: [90.0]
system/camera/vertical: [60.0]
system/bbx/x: 0.75
system/bbx/y: 0.75
system/bbx/z: 0.5
system/bbx/overshoot: 0.5

nbvp/gain/free: 0.0
nbvp/gain/occupied: 0.0
nbvp/gain/unmapped: 0.0
nbvp/gain/area: 1.0
nbvp/gain/probabilistic: 0.0
nbvp/gain/range: 5.0
nbvp/gain/zero: 0.0
nbvp/gain/degressive_coeff: 1.0
nbvp/tree/extension_range: 1.0
nbvp/tree/exact_root: true
nbvp/tree/initial_iterations: 15
nbvp/tree/cuttoff_iterations: 200
nbvp/dt: 0.1

nbvp/log/throttle: 0.25
nbvp/log/on: true

# bounding box: necessary to limit the simulation 
# scenario (smaller than actual gazebo scenario)
bbx/minX: -8.0
bbx/minY: -8.0
bbx/minZ: -0.0
bbx/maxX: 8.0
bbx/maxY: 8.0
bbx/maxZ: 3.0
bbx/softBounds: true

pcl_throttle: 0.5
inspection_throttle: 0.5

I adapted the parameters and launchfile from the Gazebo demo example, so they might be quite off / wrong.

What is the function of the system/camera/pitch parameter? I ask because the mapping component should already get the pointcloud in the camera frame, which in turn has a transform to the _baselink frame of odometry. Shouldn't the pitch be automatically computed from the base_link -> camera transform?

Thanks!

[mhkabir]

It'd also be great if you could point me to some example configuration files from a real MAV setup like from the demo on the Asctec Neo you guys flew.

[birchera]

Your parameters look actually ok. The only thing I noted, is that your octomap resolution is quite high. This might create big computational loads (scales roughly with 1/r^4). It is hard to judge where the nan values come from. It would help to identify the source if you could post the call-stack at the time the message is printed.

A parameter set that we used with the neo:

system/v_max: 0.2
system/dyaw_max: 0.4
system/camera/pitch: [15.0]
system/camera/horizontal: [90.0]
system/camera/vertical: [60.0]
system/bbx/x: 1.2
system/bbx/y: 1.2
system/bbx/z: 0.5
system/bbx/overshoot: 0.5

nbvp/gain/free: 0.0
nbvp/gain/occupied: 0.0
nbvp/gain/unmapped: 0.0
nbvp/gain/area: 1.0
nbvp/gain/probabilistic: 0.0
nbvp/gain/range: 1.5
nbvp/gain/zero: 0.0
nbvp/gain/degressive_coeff: 0.5
nbvp/tree/extension_range: 1.5
nbvp/tree/exact_root: false
nbvp/tree/initial_iterations: 20
nbvp/tree/cuttoff_iterations: 200
nbvp/dt: 0.01

nbvp/log/throttle: 0.05
nbvp/log/on: true

# bounding box: necessary to limit the simulation 
# scenario (smaller than actual scenario)
bbx/minX: -4.5
bbx/minY: -1.5
bbx/minZ: 0.5
bbx/maxX: 3.5
bbx/maxY: 7.0
bbx/maxZ: 3.0
bbx/softBounds: false

pcl_throttle: 1.0
inspection_throttle: 0.5

and

  <arg name="resolution" default="0.2" />
  <arg name="sensor_max_range" default="5.0" />

You are right about the pitch. As long as the sanity of your tf tree is given, this might be a good option.

[birchera]

@mhkabir were you able to solve this issue?

[mhkabir]

Not really. I didn't get time to work further on this issue (will get on it in around a week).

I confirmed that I have sufficient CPU idle time even with a high resolution map, and yet the TF warnings continue.

Also, could you tell me how you want me to do the stack trace on the error? GDB? On 29 Feb 2016 6:56 pm, "Andreas Bircher" notifications@github.com wrote:

@mhkabir https://github.com/mhkabir were you able to solve this issue?

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/nbvplanner/issues/2#issuecomment-190210633.

[birchera]

No problem. You probably could use GDB for that. I just thought this would make it easier for me to help you debug.

[mhkabir]

@birchera , slightly OT question - what were you using to compute stereo disparities on the Neo? Could you please share? I'm using a similar performance PC to yours and it would be nice too know how to get similar quality pointclouds as in the video.

[birchera]

Unfortunately this is ongoing research of some people here, so I cannot share this. In case you cannot find a good implementation that does the trick, you can maybe consider using a time of flight sensor?

[mhkabir]

@birchera ToF sensors aren't an option, but thanks anyway!

[mhkabir]

Ok, fixed this. It was an issue on my side. My stereo depth extraction system had been incorrectly interpolating, leading to bad data in the point cloud.

Regarding the TF warning, indeed it was due to CPU. After improving the SLAM system's computational requirements, volumetric mapping now works without a hitch.

Thanks for the help!