ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi,
The problem was caused by large covariance values (1.7976931348623157e+308
) in the Odometry messages:
$rostopic echo /odom
...
header:
seq: 7062
stamp:
secs: 1448550366
nsecs: 530036677
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: 0.118375024097
y: -0.23544191425
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.305612214835
w: 0.952156066065
covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.00191986217719
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
...
and there was a bug of double/float conversion in rtabmap node. This is now fixed in this commit.
Your bag helped me to correct another issue about variance when the link is not refined because the robot is not moving (under RGBD/LinearUpdate
and RGBD/AngularUpdate
). Thx!
I've taken demo_robot_mapping.launch to test your bag, I just did a little modification to increase the number of loop closures found by lowering the parameter LccIcp2/CorrespondenceRatio
(default 0.3, which is used to accept an ICP transformation):
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
[...]
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.2"/>
</node>
Here is the result (I also added cloud_ceiling_culling_height
parameter to filter the ceiling for better view):
cheers, Mathieu