Does obstacles_detection nodelet assume that the sensor is level?

[ibd]

Hello

I have run into yet another oddity concerning point cloud processing (referring to the "minimum height" problem in the other thread I opened).

The issue is related to the obstacles_detection nodelet. Namely, I split the cloud that I get from ROS tango streamer, from the google tango device, into an obstacles cloud and a ground cloud.

I have noticed that when I angle the tango device upwards or downwards, I get unexpected output of the obstacles_detection nodelet.

1. If I angle the point cloud up, it seems that some transformation is wrong. The output does not align with the original point cloud anymore. The rotation around the gravity seems correct, however some other rotation seems off. Here's a screenshot to illustrate:

The white point cloud is the output of the obstacles_detection nodelet, namely the obstacles cloud. The colored point cloud is the map generated by RTAB-map. As you can see, the alignment is off.

2. If I angle the point cloud down, I can see that many points are filtered -- the resulting obstacles cloud contains way less points than are observable (I am pointing the tango camera at a matte, well-lit object).


Here's a picture of the behavior if the tango tablet is held near level (not facing up or down).


All of this makes me think that the obstacles_detection nodelet assumes that the sensor input to it is level (facing straight ahead). Is that the case?
If yes, how could I change this behavior to work with angled sensor placement?

For reference, this is how I launch the obstacles_detection nodelet:

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection rtabmap_pc_processing" output="screen">
    <remap from="cloud" to="tango_point_cloud_relay"/>
    <remap from="obstacles" to="/obstacles_cloud"/>
    <remap from="ground"    to="/ground_cloud"/>

    <param name="frame_id"             type="string" value="base_footprint"/>    
    <param name="map_frame_id"         type="string" value="map"/>
    <param name="wait_for_transform"   type="bool" value="true"/>

    <param name="Grid/MaxGroundAngle"  type="int" value="20"/>
    <param name="Grid/NormalK"         type="int" value="20"/>
    <param name="Grid/MaxGroundHeight" type="double" value="0.05"/>
    <param name="Grid/CellSize" type="double" value="0.05"/>
  </node>

And this is my TF tree:




Thank you!

Best regards
ibd

[matlabbe]

frame_id is set to base_footprint. Odometry is linked to tango device, not base_footprint. Make sure the transform device -> base_footprint represents the plsition/orientation of the sensor. In that configuration, you cannot move/rotate the position of tango against the robot base as the transform is fixed.

For the height cutoff, it depends on the frame_id set. For example if it is done in device frame at 0 cm but the sensor is 30 cm high, then everything under 30 cm in map frame will be filtered.

[ibd]

Hi,

Thank you for your help.

The transform device -> base_footprint is correct. I have configured it so that the Tango tablet is sitting on the robot. This is done with a static TF publisher.
Note that this is just a visualization. I am not moving the tango device with respect to the turtlebot. The Tango tablet is sitting on my desk on initialization, and I am moving it by hand. Please ignore the turtlebot model shown on the screenshots.

Here is what the different node(let)s are using for "frame_id" and similar:
rtabmap_ros/pointcloud_to_depthimage:
"fixed_frame_id" = "odom"

rtabmap_ros/obstacles_detection:
"frame_id" = "base_footprint"
"map_frame_id" = "map"

rtabmap:
"frame_id" = "base_footprint"
"odom_frame_id" = "odom"
"map_frame_id" = "map"
"vo_frame_id" = "odom"

I am following the tutorial at this link.

The complete system works fine in the default configuration!
However, just for testing, I wanted to angle the tango device up (turtlebot visualized accidentally). Any other ideas how to fix this? I am making a video to again show the behavior a bit better.

[ibd]

Here's a video showing the behavior of obstacles_detection's output.

https://youtu.be/uJYIrd9XR1M?t=27

You can see that the point cloud does not align with the map when the Tango tablet is pointed upwards. When pointed downwards, lots of points are filtered for some reason (I can fix the height cutoff by starting the mapping with the tango on the turtlebot, near ground level, but I cannot fix the filtering of the obstacles_cloud when pointing downwards that way).

Towards the end of the video, I do a quick comparison with the raw point cloud (relay topic), to show that the original data fits in the coordinate frames well.

The fixed frame of RVIZ was set to "map".

[matlabbe]

Hi,

I did some tests with your configuration and I was able to reproduce the problems.

1) For the rotation issue, this is a bug in the code. I updated the code so that the segmented clouds are correctly transformed back to frame in the input topic. See this commit.

2) For obstacle clouds cut at some height, it is the effect of "Grid/MaxGroundHeight!=0" and "Grid/MapFrameProjection=false". Either set "Grid/MaxGroundHeight" to 0, or set "Grid/MapFrameProjection" to true.

<param name="Grid/MaxGroundHeight" type="double" value="0.05"/> 
<param name="Grid/MapFrameProjection" type="bool" value="true"/>

Details: When "Grid/MapFrameProjection" is false, "Grid/MaxGroundHeight" is relative to frame_id (e.g., base_footprint), so all points under 5 cm over where the base frame is are filtered. If "Grid/MapFrameProjection" is true, "Grid/MaxGroundHeight" is relative to map, so all points under 5 cm over the map ground (xy plane) are filtered. If "Grid/MaxGroundHeight" is 0, the results would be the same for "Grid/MapFrameProjection" set to true or false.

cheers,
Mathieu

[ibd]

Hi Mathieu

Thank you for the outstanding support!

Best
ibd