Mapping using Realsense D435 and orb_slam_2

[martinakos]

Hi,

I'm trying to create a map using the pose from ethz-asl/orb_slam_2_ros (running from a realsense D435) and the point cloud from the same D435 (/camera/depth_registered/points). I think I've connected everything fine, voxblox node spins, and repeatedly outputs

[ INFO] [1531490886.374945217]: Updating mesh.

to the terminal. However, if I look at the mesh topic the mesh_blocks are always empty:

$ rostopic echo /voxblox_node/mesh
header: 
  seq: 263
  stamp: 
    secs: 1531490808
    nsecs: 375656235
  frame_id: "world"
mesh_blocks: []
---
header: 
  seq: 264
  stamp: 
    secs: 1531490809
    nsecs: 375167789
  frame_id: "world"
mesh_blocks: []
---
...

what am I'm missing?

this is my launch file:

<launch>
  <arg name="voxel_size" default="0.05"/>
  <arg name="generate_esdf" default="true" />

   <node name="voxblox_node" pkg="voxblox_ros" type="tsdf_server" output="screen" args="-alsologtostderr" clear_params="true">
    <remap from="pointcloud" to="~camera/depth_registered/points"/>
    <param name="tsdf_voxel_size" value="$(arg voxel_size)" />
    <param name="tsdf_voxels_per_side" value="16" />
    <param name="voxel_carving_enabled" value="true" />
    <param name="color_mode" value="color" />
    <param name="use_tf_transforms" value="false" />
    <param name="update_mesh_every_n_sec" value="1" />
    <param name="min_time_between_msgs_sec" value="0.0" />
    <param name="method" value="fast" />
    <param name="use_const_weight" value="false" />
    <param name="allow_clear" value="true" />
    <param name="verbose" value="true" />
    <param name="output_mesh_as_pointcloud" value="true" />
    <remap from="transform" to="~orb_slam_2_ros_node/transform_cam" />
    <rosparam file="$(find voxblox_ros)/cfg/realsense_D435.yaml"/>
    <param name="mesh_filename" value="$(find voxblox_ros)/mesh_results/$(anon cow).ply" />
  </node>
</launch>

and the realsense_D435.yaml:

T_B_C:
- [1.0, 0.0, 0.0, 0.0]
- [0.0, 1.0, 0.0, 0.0]
- [0.0, 0.0, 1.0, 0.0]
- [0.0, 0.0, 0.0, 1.0]
invert_T_B_C: false

T_B_D:
- [ 1.0, 0.0, 0.0, 0.0]
- [ 0.0, 1.0, 0.0, 0.0]
- [ 0.0, 0.0, 1.0, 0.0]
- [ 0.0, 0.0, 0.0, 1.0]
invert_T_B_D: false

I admittedly don't really know if these transforms are. with the realsense I use aligned color and depth frames so I assume I can put identities for these and at least see something in rviz, even if wrong.

[ZacharyTaylor]

The mesh update happens periodically even if no new data has been received. Everything appears to be hooked up correctly so I can only think of four possibilities

1) The pointcloud isn't being input correctly On my D415 the pointcloud is always output on the topic /depth/color/points rather than ~camera/depth_registered/points assigned here so I thought I would just double check that is correct

2) The transform isn't being accepted for some reason. You could try switch to using TFs instead as I believe that orb slam outputs these.

3) Some timing issue. This is unlikely as I believe it should be throwing lots of warnings and errors. However, if running off a bag make sure you use --clock. Otherwise I remember disliking the way the realsense timestamps its data enough to rip it out and replace it in our own driver (which also has some more speckle filtering to help give nice 3D reconstructions). https://github.com/ethz-asl/realsense/pull/28

4) Some internal bug. Building in debug enables a bunch more checks and while it will be too slow to run real-time might give more hints as to the issue.

[martinakos]

Thanks for the reply. I imagine that the pointcloud topic depends on what launch file is the realsense2_camera node launched with. I'm just using the rs_rgbd.launch that comes with the realsense2_camera If I remove the ~ in front of the topic, ie. /camera/depth_registered/points I get slight different messages in the terminal, but still no mesh_blocks being output that I can see in rviz. The terminal output in that case looks like this:

[ INFO] [1531733613.030664531]: Layer memory: 20
[ INFO] [1531733613.061639432]: Timings: 
SM Timing
-----------
mesh/publish        764 00.006687   (00.000009 +- 00.000004)    [00.000005,00.000038]
mesh/update         764 00.365803   (00.000479 +- 00.000311)    [00.000154,00.006018]

[ INFO] [1531733613.061676972]: Layer memory: 20
[ INFO] [1531733613.101494849]: Timings: 
SM Timing
-----------
mesh/publish        764 00.006687   (00.000009 +- 00.000004)    [00.000005,00.000038]
mesh/update         764 00.365803   (00.000479 +- 00.000311)    [00.000154,00.006018]

I'm not sure what you mean with switching to use TFs. The orb_slam2 I'm running only publishes orb_slam_2_ros_node/transform_cam which is of type geometry_msgs/TransformStamped and that's the type of topic that voxblox seems to expect.

As for the other points you mention. I think I'm going to have to learn more ROS before being able to debug the node.

[helenol]

This means that the message isn't hooked up correctly -- there's never a callback for the pointcloud.

run: rosnode info your_node_name and check under connections: is there something connected to the pointcloud topic?

On Mon, Jul 16, 2018 at 11:49 AM martinakos notifications@github.com wrote:

Thanks for the reply. I imagine that the pointcloud topic depends on what launch file is the realsense2_camera node launched with. I'm just using the rs_rgbd.launch that comes with the realsense2_camera https://github.com/intel-ros/realsense If I remove the ~ in front of the topic, ie. /camera/depth_registered/points I get slight different messages if the terminal, but still no mesh_blocks being output that I can see in rviz. The terminal output in that case looks like this:

[ INFO] [1531733613.030664531]: Layer memory: 20

SM Timing

mesh/publish 764 00.006687 (00.000009 +- 00.000004) [00.000005,00.000038] mesh/update 764 00.365803 (00.000479 +- 00.000311) [00.000154,00.006018]

[ INFO] [1531733613.061676972]: Layer memory: 20

SM Timing

mesh/publish 764 00.006687 (00.000009 +- 00.000004) [00.000005,00.000038] mesh/update 764 00.365803 (00.000479 +- 00.000311) [00.000154,00.006018]

I'm not sure what you mean with switching to use TFs. The orb_slam2 I'm running only publishes orb_slam_2_ros_node/transform_cam which is of type geometry_msgs/TransformStamped and that's the type of topic that voxblox seems to expect.

As for the other points you mention. I think I'm going to have to learn more ROS before being able to debug the node.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/ethz-asl/voxblox/issues/184#issuecomment-405197458, or mute the thread https://github.com/notifications/unsubscribe-auth/AFWzCO3H7b0IISk5Fv7yiBFnJmDn0zKJks5uHGGxgaJpZM4VO9T1 .

[martinakos]

This is the output of rosnode info:

--------------------------------------------------------------------------------
Node [/voxblox_node]
Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /voxblox_node/mesh [voxblox_msgs/Mesh]
 * /voxblox_node/occupied_nodes [visualization_msgs/MarkerArray]
 * /voxblox_node/surface_pointcloud [sensor_msgs/PointCloud2]
 * /voxblox_node/tsdf_map_out [voxblox_msgs/Layer]
 * /voxblox_node/tsdf_pointcloud [sensor_msgs/PointCloud2]
 * /voxblox_node/tsdf_slice [sensor_msgs/PointCloud2]

Subscriptions: 
 * /camera/depth_registered/points [sensor_msgs/PointCloud2]
 * /orb_slam_2_ros_node/transform_cam [geometry_msgs/TransformStamped]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]
 * /voxblox_node/tsdf_map_in [unknown type]

Services: 
 * /voxblox_node/clear_map
 * /voxblox_node/generate_mesh
 * /voxblox_node/get_loggers
 * /voxblox_node/load_map
 * /voxblox_node/publish_map
 * /voxblox_node/publish_pointclouds
 * /voxblox_node/save_map
 * /voxblox_node/set_logger_level

contacting node http://mtlinux:34883/ ...
Pid: 1626
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /tf
    * to: /orb_slam_2_ros_node (http://mtlinux:35123/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf_static
    * to: /camera/realsense2_camera_manager (http://mtlinux:40441/)
    * direction: inbound
    * transport: TCPROS
 * topic: /orb_slam_2_ros_node/transform_cam
    * to: /orb_slam_2_ros_node (http://mtlinux:35123/)
    * direction: inbound
    * transport: TCPROS
 * topic: /camera/depth_registered/points
    * to: /camera/realsense2_camera_manager (http://mtlinux:40441/)
    * direction: inbound
    * transport: TCPROS

[martinakos]

It seems the ROS nodes were correctly connected. However, the pairing between poses and pointclouds (as happening in TsdfServer::processPointCloudMessageAndInsert) happened very infrequently. I changed the launch parameters to try to speed up the point cloud integration and use 10 for pointcloud_queue_size. With this I could see the mesh being updated in RViz every a few seconds.

There is another problem though. Voxblox is using many cores and with the camera and OrbSLAM2 nodes working I have my 8 cores 100%. This results in OrbSLAM2 slowing down and sometimes losing track. I get the feeling that when OrbSLAM2 slows down wrt. to the camera node the pairing of poses and pointclouds gets worse. Ideally I'd like to speedup Voxblox as much as possible but I also don't want to slow down other nodes. How could I run voxblox single-threaded?

[ZacharyTaylor]

This line sets how many cores voxblox will use https://github.com/ethz-asl/voxblox/blob/master/voxblox/include/voxblox/integrator/tsdf_integrator.h#L49

However, I feel restricting voxblox to one core won't solve your issues as it will still drop behind realtime. There are quite a few parameters that make a large difference to the CPU usage of voxblox. I would first suggest trying the following:

• increasing tsdf_voxel_size
• decreasing max_ray_length_m
• Switching method to fast
• If you don't care about freespace try disabling voxel_carving_enabled
• Finally to force the system to be realtime give an upperbound for the runtime per frame with max_integration_time_s

[martinakos]

Thanks for the help. I've tuned the parameters you suggest and got some reasonable scans. I also run voxblox with only one thread. Now I see that OrbSLAM2 likes to slow down from 30fps to 22fps regardless of how much cpu/cores are still available. I guess that's something to investigate with the OrbSLAM2 project.

Making a scan of a room is just the first step I had to try. Actually what I'm trying to do is to create individual tsdfs for objects that I detect with a mask-rcnn. I would only integrate points within the frustum of the object mask, and I hope I can reduce the voxel size enough for each tsdf so that I can capture some detail in the objects. Do you have any suggestions on how to do this with voxblox? or whether voxblox is the most suitable library to achieve this particular task?

[JiaruiWang-Jill]

hi, @martinakos

Sorry to bother but I am also trying to use orbslam2 to get pose, then combine voxblox with orbslam2 to generate real time mesh in a CPU based PC.

I already installed orb_slam_ros and this package. I am totally new on ROS thing and I fail to know how to put these two together. Could you give more hints on this?

Thanks a lot in advance!

[MaEppl]

Hey I'm also using D435+orbslam2+voxblox. But the results are noisy. I'm using orbslam in stereo mode and use the depth picture of the D435 as input for voxblox. I enabled the laser emitter for better depth pictures in low textured environments. But then the laserpattern is recognized as ORB feautures, and the calculated trajectory is wrong. Are you all using stereo mode? I wanted to use the aligned_depth and rgb topic for using the rgbd slam mode (in the rgb picture there is no laser pattern). But the camera position jumps back and forth, I'm not sure about the baseline parameter in this case.

[ZacharyTaylor]

When we have made use of a realsense + orbslam2 + voxblox we have generally sidestepped the issue by using a separate visible spectrum stereo camera for orbslam and only used the realsense for depth estimation. The color images of the realsense are rolling shutter and not synced so I wouldn't recommend using them for state estimation.

[rancheng]

does pointcloud frame_id need to set to world?

[shabpompeiano]

Adding this to the launch file made it work for me:

<param name="use_tf_transforms" value="true" />
<remap from="transform" to="/tf" />
<param name="world_frame" value="world" />
<param name="sensor_frame" value="camera" />