Multiple and flipped octomaps being generated
Hi everyone,
So as a part of my GSoC 2018 project, I am using the Kinect Fusion algorithm for the process of 3-D reconstruction. Initially, I am launching the robot in Gazebo and the robot in RViz! with MoveIt!. I have placed a random sphere object for visualization in Gazebo.
After this, I launch the Kinect Fusion reconstruction node which utilizes a TSDF based approach for Fusion using the YAK package. However, as soon as this file is launched, after a time of 2-3 seconds, I get this one warning on the RViz console.
[ WARN] [1531932207.525024167, 3837.520000000]: No transform available between frame 'map' and planning frame '/world' (Could not find a connection between 'world' and 'map' because they are not part of the same tree.Tf has two or more unconnected trees.)
The Kinect fusion node console shows the following:
[ INFO] [1531932206.867190280]: starting kinfu node...
Device 0: "GeForce GTX 960M" 4044Mb, sm_50, 640 cores, Driver/Runtime ver.9.0/8.0
[ INFO] [1531932206.886842801]: Volume size is 1.568 0.768 0.384
[ INFO] [1531932207.009896212]: Fixed frame: /map Camera frame: /camera_depth_frame
[ INFO] [1531932207.029919344]: Use pose hints set to 1
[ERROR] [1531932207.172164447, 3837.196000000]: Lookup would require extrapolation into the past. Requested time 3837.187000000 but the earliest data is at time 3837.191000000, when looking up transform from frame [camera_depth_frame] to frame [volume_pose]
[ INFO] [1531932207.172317940, 3837.196000000]: Connecting to camera...
[ INFO] [1531932207.226409163, 3837.246000000]: Connected.
bilateral_sigma_depth: 0.04
bilateral_sigma_spatial: 4.5
gradient_delta_factor: 0.5
icp_angle_thresh: 0.523599
icp_dist_thresh: 0.1
icp_truncate_depth_dist: 0
raycast_step_factor: 0.05
tsdf_min_camera_movement: 0.0001
tsdf_trunc_dist: 0.005
volume_resolution: 0.001
[ INFO] [1531932207.247908859, 3837.264000000]: volPos (loaded): 0, 0, 0
[ INFO] [1531932207.320968639, 3837.324000000]: Volume size set to: [1.568, 0.768, 0.384]
[ INFO] [1531932207.321760993, 3837.324000000]: Volume pose set to: [1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, 0;
0, 0, 0, 1]
Reset Volume
[ INFO] [1531932207.330725281, 3837.332000000]: Starting tracking...
[ERROR] [1531932207.969904398, 3837.948000000]: Could not find a connection between 'volume_pose' and 'camera_depth_frame' because they are not part of the same tree.Tf has two or more unconnected trees.
[ INFO] [1531932207.972523366, 3837.948000000]:
Motion Input: [0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 1]
Motion Corrected: [0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 1]
Pose Input: [0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 1]
Pose Corrected: [0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 0;
0, 0, 0, 1]
The same Octomap is flipped to the right side. This happens consistently on n number of trials.
I have read in some posts that a static publisher should be used in order to avoid the error.
The Kinect fusion launch file is creating a static transform publisher like this.
<node name="map_to_volume_publisher" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 ...