Troubleshooting mapping with kinect v2, rplidar and jetson tx2

[Ross_L]

Hello

I hope you can help please. I am looking for some suggestions on where to start looking to troubleshoot my robot to work with rgbd mapping. At the moment I am getting a lot of ghosting and smearing, I am guessing due to a lack of loop closures.



My setup is:

Nvidia Jetson TX2
Client PC running rtabmap (GTX 980, i7 5820K)
Kinect V2 running with kinect2_bridge (I am not 100% sure if its working with CUDA or OpenCL)
Roboclaw motor driver which supplies the /odom topic via a python node
2 x servocity dc brushed motors with quadrature encoders
RPLidar A1 publishing to /scan

I have attached my launch files including the bringup and rtabmap launch files
gbot_bringup.launch
rtabmap.launch

I am using the stock kinect2_bridge launch file.

I can view the laser data in the RVIZ window.

I have confirmed that odometry and the laser scan work by testing with gmapping, which produces a good map. The robots movements appear to be accurately represented in RVIZ. I've read through some of the parameter tweaking docs and looked through the forum to see if I can glean some insights, but it would be great if anything seems obvious that I have missed before hunkering down for "tweaking mode"!

Something of concern is whether I am having bandwidth issues. When running the bringup file, the TX2 shows that 40 MiB/s are going across the network. That is going over my standard home wifi. That sounds like a lot! I am running the throttle nodelet. Should the figure be this high?

One of the behaviours I notice is that it takes a long time, sometimes a minute or so, for the 2d map (occupancy map?) to pop up in RVIZ, subscribed to the /map topic. As mentioned it also cannot create an accurate 2d map and suffers the same smearing as the 3d map.

Any pointers would be super appreciated!

Cheers


EDIT: Changed rtabmap launch file as I uploaded the wrong one

[Ross_L]

Correction: Network usage is 25 MiB/s, not 40!

EDIT: Tried changing the bringup params for kinect2_bridge to sd from qhd and added decimation = 3, still does not seem to make any difference

EDIT2: Ah ok, it looks like I was subscribing to the non throttled data in RVIZ, and instead was pulling the regular sd depth stream. That said, now I clicked on the "data throttled" entry, I am getting nothing at all! Bit more digging I think.

Should the standard sd stream really be 25 MiB second though?

EDIT3: Ok I have managed to get an SD throttled stream working, its about 8-10 MiB/s with transport compressedDepth. Rtabmap looks a bit happier and is now giving messages about looking for inliers. I am still getting the smearing though, so back to original question, what would be the best parameters to check first? I note from the remote mapping tutorial the bandwidth figure should be around 500 KiB/s, compared with my 8-10 MiB/s.

Thanks!

[Ross_L]

Just a latest shot. This is a square room:



I am a bit concerned about the black edging on the depth cloud, which looks like an uncalibrated cloud However the kinect has been calibrated, at least within the kinect2_bridge package. Should it be calibrated within rtabmapviz? Unfortunately the 'source' menu is grayed out when I run rtabmapviz, so not able to test at present. Any thoughts?

There also seems to be a lot of black 'noise' that only appears in the depth cloud when rtabmap is running.

EDIT: ntpdate output:

server 192.168.1.99, stratum 2, offset 0.015392, delay 0.05731
17 Feb 11:13:05 ntpdate[7327]: adjust time server 192.168.1.99 offset 0.015392 sec

Is a hundredth-or-so of a second acceptable here?

Also max depth is set to 4

[Ross_L]

Ok so I wondered if maybe it was a performance problem on the TX2 and so built the robot package on an old gaming laptop. Because of the combination of optimus graphics being fussy about driver, CUDA's proclivities towards GCC versions and a host of other annoyances, I have to run kinect2_bridge with opengl and CPU registration. Not ideal, but definitely faster than the TX2. However, this only improves the response time of mapping and not the lining up of images, leading me to think its not a performance problem. That brings me back to the black borders on everything!

I am wondering - if you move the kinect2 and mount it in a different way, do you need to rerun calibration / extrinsics? Looking at those black borders on everything makes me think this is the case. The original calibration was done on a tripod with a flat angle to the kinect2, but now on the robot it is looking up at 15 degrees. Should I rerun calibration?

Additionally is there some kind of calibration that must be done for lidar / odometry, or does the URDF somehow take care of all that?

Thanks for reading this far and apologies for making you come back to a novel!

[matlabbe]

Can you share the rtabmap.db? (Located by default in ~/.ros/rtabmap.db) It could be useful before digging in the possible problems.

[Ross_L]

Here's the re-upload, apologies as I loaded the wrong one before. Its zipped as was a couple of hundred megabytes

https://drive.google.com/file/d/1FgIwR79r_L2iemRPTBJrCTycsxv7fzXo/view?usp=sharing

It seems that the live display in rviz makes things look even worse, and when you redownload the graph and map, or view in database viewer, there is a small improvement. As you can see though there is still a lot of alignment issues and the black smearing / noise, some of which appears to relate to the differing FOV of the depth and RGB cams (I think).

I really would love to get this bot mapping properly :)

Cheers

EDIT: Whilst I think about it, I am pretty sure that my URDF reflects the robot, as most of the parts were made in CAD and 3D printed, including the drive wheels which have rubber tyres, petg hubs and lathe-turned aluminium couplings. In terms of odometry, I am not using any fancy ros-control stuff, its just the odometry being fed from the motor encoders, to the roboclaw, and then via roboclaw-ros node to the /odom topic.

There is a caster at the front of the bot, the type that swivels.

[Ross_L]

I now believe it may be an odometry issue. I noticed that one of my wheel encoders produces clicks in the ratio of 0.93 to the other. If I command them both to move a set number of clicks, one wheel always moves slightly more than the other. This also seems to translate into a tendency to veer to one side when a simple "drive forward" command is issued.

I have tuned the PID using the motion control studio program for the roboclaw, but this has no effect on the offset.

I am talking to the manufacturer to find out how to fix this, but in the meantime I am going to try using visual odometry. In order to do so, do I simply change the launch file to reflect visual odometry = true, and then modify the motor driver node to stop publishing odometry? Does anything need to be altered in this part of the launch file?

      <!-- Basic RTAB-Map Parameters -->
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="robot_footprint"/>
      <param name="odom_frame_id"       type="string" value="odom"/>
      <!--param name="subscribe_depth"     type="bool"   value="true"/-->
      <param name="subscribe_scan"      type="bool"   value="true"/>
     <param name="subscribe_odom"      type="bool"   value="true"/>
      <param name="visual_odometry"      type="bool"   value="true"/> <!--changed this to true-->
      <param name="odom_topic"      value="/odom"/>
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
          <param name="queue_size" type="string" value="20"/>

Apologies for the frequent posts, I am working on this daily to get it fixed!

EDIT: Nope, no dice. I remapped the name of the roboclaw topic to /roboclaw/odom in its launch file, then set visual_odometry to true in the rtabmap file. I am assuming it was then using visual odometry? The 2D ground map as before seems to generate ok but the same issue with the rgbd, it just does not seem to want to work. Same choppy images and does not seem to track with the ground map as the robot moves.

Am beginning to lose hair over this, any help you can give would be massively appreciated :D

[matlabbe]

Hi,

I took a look at the database.

Scan matching seems working good for most of the mapping, but when the robot is rotating, wheel odom seems to drift more and scan matching has more difficulty to correct it. Increasing Icp/MaxCorrespondenceDistance from 0.05 to 0.1 helped in some case.

Maybe decrease the rotation speed to make scan matching easier (unless you can get more accurate odometry on rotation).

When the robot is rotating, the RGB and depth images are poorly synchronized, this will make the 3D map looking bad and loop closures won't be as accurate in those situations. However, in your case you are using a lidar, which corrects most of the not-so-accurate loop closures computed by the camera.

I recommend to use "qhd" quality for the kinect to get depth image registered to color camera, not color registered to depth image like in "sd" mode (this causes a lot of black pixels in color images, which affects feature extraction). Using "qhd" will also help if you use visual odometry (look at this kinectv2 example launch file for some tuning parameters for qhd images).

For the screenshot of your first post, the RVIZ global frame should be "map", not "odom", otherwise the rtabmap_ros/MapCloud display won't work correctly (like adding clouds over the same place).

cheers,
Mathieu

[Ross_L]

Thanks for taking a look at the database!

Can you explain where the "Icp/MaxCorrespondenceDistance" value belongs please? This is my current launch file:

rtabmap.launch

I will confess to being confused with all the many parameters and sub modules! I see "Icp/MaxCorrespondenceDistance" has something to do with Icp odometry, is this the same thing that has been set to true with the

<param name="visual_odometry"      type="bool"   value="true"/>

line, and so is just an added parameter line, or should I be separately launching icp odometry somehow? If so where would that appear in my current launch file please?

Additionally, for the robot's launch file, I followed a reply from you on the forum here that said for the kinect2_bridge node, the "approx_sync" parameter should be false as kinect2_bridge has synced images. Is this correct? I do not know of a reason why my rgb and depth images should be out of sync when rotating, I am turning the robot extremely slowly using the xbox360 teleop.

I have switched to map frame but am sadly still getting ghosted images.

I have also switched over to an old gaming laptop with an older i7 as the tx2 just could not keep up with the qhd. However even though when I use rostopic hz on the laptop, all the depth streams etc are publishing fine with almost no "no new messages", when doing to same from the client computer I am getting frequent "no new messages" and after a minute or so, the map just stops updating. Is it possibly a network issue?

Using qhd does seem to have corrected the black spots though! Thank you

[matlabbe]

Hi,

You are not using visual or icp odometry in your launch file, you are using odometry from the robot (wheel odometry).

Documentation about RTAB-Map's parameters can be printed with:

$ rosrun rtabmap_ros rtabmap --params | grep Icp/MaxCorrespondenceDistance
Param: Icp/MaxCorrespondenceDistance = "0.1"               [Max distance for point correspondences.]

Icp/MaxCorrespondenceDistance is used for refining loop closure detection and do proximity detection. When doing scan matching, only points under this distance are considered for ICP.

To set in your launch file, add the following under rtabmap node:

<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

Here is an example of poor synchronization between rgb and depth:


Yes, approx_sync should be false. The problem is that in your launch file, we should pre-sync kinect image because you are also subscribing to a scan topic (coming from a different sensor). This can be done like in figure 6 of this paper, using rgbd_sync node. In your launch file, this would look like this:

<?xml version="1.0" encoding="UTF-8"?>

<launch>
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgb_topic"         default="/kinect2/qhd/image_color_rect"/>
  <arg name="depth_topic"       default="/kinect2/qhd/image_depth_rect"/>
  <arg name="camera_info_topic" default="/kinect2/qhd/camera_info"/> 

  <!-- Mapping Node -->
  <group ns="rtabmap">

     <!-- pre-sync kinect images before rtabmap node -->
     <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
      <remap from="depth/image"       to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
      <remap from="rgbd_image"        to="rgbd_image"/> <!-- output -->
      <param name="approx_sync"       value="false"/>   <!-- exact sync for kinect2 topics -->
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

      <!-- Basic RTAB-Map Parameters -->
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="robot_footprint"/> <!--"robot_footprint"/-->
      <param name="odom_frame_id"       type="string" value="odom"/>
      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_rgbd"      type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="subscribe_odom"      type="bool"   value="true"/>
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="queue_size" type="string" value="20"/>

      <!-- RTAB-Map Inputs -->
      <remap from="scan"       to="/scan"/>
      <remap from="rgbd_image" to="rgbd_image"/>

      <!-- RTAB-Map Output -->
      <remap from="grid_map" to="/map"/>

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/> 

      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="string" value="true"/>      

      <!-- Loop Closure Detection -->
      <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="Kp/DetectorStrategy" type="string" value="3"/> 

      <!-- Maximum visual words per image (bag-of-words) -->
      <param name="Kp/MaxFeatures" type="string" value="500"/>  

      <!-- Used to extract more or less SURF features -->
      <param name="SURF/HessianThreshold" type="string" value="150"/>

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="1"/> 

      <!-- Minimum visual inliers to accept loop closure -->
      <param name="Vis/MinInliers" type="string" value="12"/> <!--10-->

      <!-- Set to false to avoid saving data when robot is not moving -->
      <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>

      <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

    </node> 
  </group>

  <!--launch rviz-->
  <node name="rviz" pkg="rviz" type="rviz" respawn="false"/>
</launch>

cheers,
Mathieu

[Ross_L]

Hi

Using your launch file, there are now significant improvements, thank you!  I reduced the forward and rotation speeds to about 0.15 in the teleop node, and changed the frame to map in rviz. I also realised I had some parameters set within RVIZ' mapcloud section regarding update frequency and voxel size, which I altered to 0, 0 and 0.008 respectively. I also switched back to using SURF detection.

I noticed in your suggested launch file you used the following:

  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgb_topic"         default="/kinect2/qhd/image_color_rect"/>
  <arg name="depth_topic"       default="/kinect2/qhd/image_depth_rect"/>
  <arg name="camera_info_topic" default="/kinect2/qhd/camera_info"/> 

rather than the throttled topics, does this have an advantage over the non throttled?

Unfortunately as you will see, I am still seeing a lot of mismatching, though the map is a lot more recognisable.

Would using visual odometry help, if the wheel odom is out? I am not sure how to prevent the launch file using the wheel odom as I assumed you just changed the visual_odometry parameter to true. I do not think I am going to be able to crrect the wheel odom until I have a new motor, as it appears there is a hardware problem with one of the wheel encoders. It always reports 40 clicks per turn more on one of the wheels.

The map also seems to stop updating after a time, I am not sure why as rostopic hz appears to show the clouds still publishing.

Here is my new database:

https://drive.google.com/file/d/1X44Hit6F1J_h2mfQJ0vOMjfA5Wmwmz-B/view?usp=sharing

Excuse the mess in the house, what with all this roboting I've not had time to tidy up!

EDIT: Just to check - I took the launch parameters for the kinect throttled topics from this page: https://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping and used the following:

  <!-- Use same nodelet used by Freenect/OpenNI -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node>
  </group> 

I realise now that is for the original kinect, should it be different for the kinect 2 with kinect2_bridge?

[matlabbe]

Hi,

I wasn't sure from where the throttled topics were coming from. I re-read your first post, and I see that you are running kinect2_bridge and lidar on the jetson, and rtabmap on remote computer. In that kind of setup, rgbd_sync should be launched on the jetson. With kinect2_bridge, I suggest to launch it by setting the fps_limit parameter instead of throttling the images. Otherwise throttling could be combined with rgbd_sync on the kinect2_bridge nodelet manager for efficiency on the jetson:

<launch>
  <arg name="rate"     default="10"/>
  <arg name="decimation"     default="1"/>
  <!-- Use same nodelet used by kinect2_bridge -->
  <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle kinect2" output="screen">
      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="false"/> <!-- exact sync for kinect2 topics -->

      <remap from="rgb/image_in"       to="/kinect2/qhd/image_color_rect"/>
      <remap from="depth/image_in"     to="/kinect2/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info_in" to="/kinect2/qhd/camera_info"/>

      <remap from="rgb/image_out"       to="/kinect2/qhd/data_throttled_image"/>
      <remap from="depth/image_out"     to="/kinect2/qhd/data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="/kinect2/qhd/data_throttled_camera_info"/>
  </node>

  <!-- pre-sync kinect images before sending them over network -->
  <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync kinect2">
      <remap from="rgb/image"         to="/kinect2/qhd/data_throttled_image"/>
      <remap from="depth/image"       to="/kinect2/qhd/data_throttled_image_depth"/>
      <remap from="rgb/camera_info"   to="/kinect2/qhd/data_throttled_camera_info"/>
      <remap from="rgbd_image"        to="/rtabmap/rgbd_image"/> <!-- output -->
      <param name="approx_sync"       value="false"/>   <!-- exact sync for kinect2 topics -->
  </node>
</launch>

Then as rtabmap is on remote computer, we should subscribe to compressed rgbd_image topic for bandwidth efficiency (if it is connected on LAN, you may subscribe to raw topic to save some compression time on the jetson):

<launch>
  <arg name="database_path"     default="rtabmap.db"/>

  <!-- Mapping Node -->
  <group ns="rtabmap">

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

      <!-- Basic RTAB-Map Parameters -->
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="robot_footprint"/> <!--"robot_footprint"/-->
      <param name="odom_frame_id"       type="string" value="odom"/>
      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_rgbd"      type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="subscribe_odom"      type="bool"   value="true"/>
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="queue_size" type="string" value="20"/>

      <!-- RTAB-Map Inputs -->
      <remap from="scan"       to="/scan"/>
      <remap from="rgbd_image" to="rgbd_image/compressed"/>

      <!-- RTAB-Map Output -->
      <remap from="grid_map" to="/map"/>

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/> 

      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="string" value="true"/>      

      <!-- Loop Closure Detection -->
      <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="Kp/DetectorStrategy" type="string" value="3"/> 

      <!-- Maximum visual words per image (bag-of-words) -->
      <param name="Kp/MaxFeatures" type="string" value="500"/>  

      <!-- Used to extract more or less SURF features -->
      <param name="SURF/HessianThreshold" type="string" value="150"/>

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="1"/> 

      <!-- Minimum visual inliers to accept loop closure -->
      <param name="Vis/MinInliers" type="string" value="12"/> <!--10-->

      <!-- Set to false to avoid saving data when robot is not moving -->
      <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>

      <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

    </node> 
  </group>

  <!--launch rviz-->
  <node name="rviz" pkg="rviz" type="rviz" respawn="false"/>
</launch>

I looked at the database, the 2D map looks good, only thing remaining is the poor rgb/depth synchronization. With rgbd_sync running on jetson like above, I think this will help.

cheers,
Mathieu

[Ross_L]

Hi

I am battling with a "Did not receive data since 5 seconds!" error that I cannot fix. I am usually quite good at correcting them but this one has me beaten! I am using the exact launch parameters you listed above, but keep getting:

[ WARN] [1551459623.575072506]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") 
and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). 
If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/rgbd_image/compressed,
   /scan

I am running a chrony server and clocks are reported by ntpdate -q as being 0.002 seconds apart.

When I rostopic echo rgbd_image/compressed, I occasionally note that all the header data is empty (Edit: no longer the case, echoing /rgbd_image/compressed/header shows time stamps are present). Is this something to do with the setup of rgbd_sync? rostopic hz reports messages received just fine:

ross@ROSUbuntu:~$ rostopic hz /rgbd_image/compressed 
subscribed to [/rgbd_image/compressed]
average rate: 2.515
	min: 0.398s max: 0.398s std dev: 0.00000s window: 2
average rate: 2.324
	min: 0.398s max: 0.463s std dev: 0.03272s window: 3
average rate: 2.037
	min: 0.398s max: 0.741s std dev: 0.12663s window: 6
average rate: 2.196
	min: 0.272s max: 0.741s std dev: 0.13094s window: 8
average rate: 2.081
	min: 0.272s max: 0.741s std dev: 0.13381s window: 10
average rate: 2.190
	min: 0.241s max: 0.741s std dev: 0.13313s window: 13
average rate: 2.259
	min: 0.241s max: 0.741s std dev: 0.13514s window: 15
average rate: 2.238
	min: 0.241s max: 0.741s std dev: 0.12689s window: 17
average rate: 2.249
	min: 0.241s max: 0.741s std dev: 0.11828s window: 20
average rate: 2.234
	min: 0.241s max: 0.741s std dev: 0.11310s window: 22
average rate: 2.235
	min: 0.241s max: 0.741s std dev: 0.10823s window: 24
average rate: 2.225
	min: 0.241s max: 0.741s std dev: 0.10406s window: 26
average rate: 2.229
	min: 0.241s max: 0.741s std dev: 0.10026s window: 28
^Caverage rate: 2.224
	min: 0.241s max: 0.741s std dev: 0.09699s window: 30

ross@ROSUbuntu:~$ rostopic hz /scan
subscribed to [/scan]
average rate: 7.085
	min: 0.135s max: 0.145s std dev: 0.00448s window: 7
average rate: 7.060
	min: 0.135s max: 0.146s std dev: 0.00397s window: 14
average rate: 7.041
	min: 0.135s max: 0.146s std dev: 0.00360s window: 21
average rate: 7.046
	min: 0.135s max: 0.146s std dev: 0.00365s window: 28
average rate: 7.061
	min: 0.132s max: 0.146s std dev: 0.00398s window: 35
average rate: 7.055
	min: 0.132s max: 0.146s std dev: 0.00385s window: 42
average rate: 7.057
	min: 0.132s max: 0.148s std dev: 0.00388s window: 49
average rate: 7.058
	min: 0.132s max: 0.148s std dev: 0.00393s window: 57
average rate: 7.057
	min: 0.132s max: 0.148s std dev: 0.00398s window: 64
average rate: 7.054
	min: 0.132s max: 0.148s std dev: 0.00397s window: 71
average rate: 7.056
	min: 0.132s max: 0.148s std dev: 0.00391s window: 78
^Caverage rate: 7.052
	min: 0.132s max: 0.148s std dev: 0.00386s window: 81

 
Here are my launch files again:
On the jetson:
ghettobot_bringup.launch

On the remote PC:
rtabmap.launch

Edit: NB I have played around with the topic names since posting the latter, at the moment I have

<remap from="rgbd_image" to="/rgbd_image/compressed"/>

 to prevent the prepending of /rtabmap to the topic name

All the data is being sent over a standard home wifi connection via the standard jetson tx2 wifi / antenna.

[Ross_L]

OK this is definitely intermittent, not able to get to the root of why at the moment. As mentioned there is a chrony sever running on the remote PC and the jetson takes its time from that, though I am wondering if something is going on and the two lose connection. Is there some way to monitor this?

Rtabmap will map for a few moments, and then stop, then suddenly start again. It seems this stopping and starting is what introduces the mismatching.

I am at a loss how to troubleshoot this as is way outside my coding experience level, could you suggest possible log files etc to check? roswtf gives nothing obvious.

[matlabbe]

Hi,

In my example, rgbd_sync is publishing under "/rtabmap/rgbd_image" topic, so rtabmap should subscribe to "/rtabmap/rgbd_image/compressed" to get the compressed topic over the network.

When you do "$ rostopic hz /rtabmap/rgbd_image/compressed", you actually open a new image streaming, which can affect bandwidth. You may compare between "rostopic hz /rtabmap/rgbd_image/compressed" done on jetson vs remote computer and see if there is a frame rate difference. If so, it is a bandwidth problem.

Is rtabmap running over WIFI or LAN? Can you show network usage on remote computer (System Monitor)?

When you say that the header is not always set in rgbd_image topic, do you mean if you do

$ rostopic echo /rtabmap/rgbd_image/compressed/header

the stamp and frame_id are sometime null?

cheers,
Mathieu

[Ross_L]

To answer your question, I am using wifi, however I also tested using a LAN cable. No improvement, and as I found out this was not the issue. I never saw a repeat of the empty header so I have no idea what happened there! :)

Instead I increased the queue size to 50 and it now happily updates continuously. I had tried changing this before but had "type" as string when it should be int and so it did not work.

One strange thing though, it refuses to work with rtabmap/rgbd_image/comrpessedas you mention above, and will only work if rtabmap is subscribing to /rgbd_image/compressed. Does this matter or is it fine as long as its receiving rgbd_image/compressed in one form or another?

Sadly, the map still has the same problems as before, if not more so after pre-syncing on the jetson. RGB and depth images seem to be mapped to the wrong things, so for example a table image being mapped onto a wall or blue sky from a window being mapped onto a window frame.  NB the kinect has been calibrated via the kinect2_bridge calibration tool and the calibration file put in the correct folder.




Here is a sample database:

https://drive.google.com/file/d/1KZHv3ize2LI5DoFg6LTD2BOwAJDX9UEW/view?usp=sharing

Could this still be caused by the discrepancy in the odometry I mentioned before? I have one wheel reporting 40 counts per rev more than the other due to a hardware problem and am waiting for a new motor.

[matlabbe]

For the topic name /rgbd_image/compressed, maybe I typed wrong the output remapping in this code:

<!-- pre-sync kinect images before sending them over network -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync kinect2">
    <remap from="rgb/image"         to="/kinect2/qhd/data_throttled_image"/>
    <remap from="depth/image"       to="/kinect2/qhd/data_throttled_image_depth"/>
    <remap from="rgb/camera_info"   to="/kinect2/qhd/data_throttled_camera_info"/>
    <remap from="rgbd_image"        to="/rtabmap/rgbd_image"/> <!-- output -->
    <param name="approx_sync"       value="false"/>   <!-- exact sync for kinect2 topics -->
</node>

Make sure that approx_sync is false in both data_throttle and rgbd_sync. To make sure its the correct images that are synchronized, you may even echo the stamp for those topics and see if they have all the same stamps:

$ rostopic echo /kinect2/qhd/data_throttled_image/header/stamp
$ rostopic echo /kinect2/qhd/data_throttled_image_depth/header/stamp
$ rostopic echo /kinect2/qhd/data_throttled_camera_info/header/stamp

The registration between color and depth seems okay when not moving. The registration is independent of the odometry. IF the stamps are equal between color and depth image and they don't match when the robot is moving, this is mostly an issue coming from kinect2_bridge that is not able to process enough fast the frames coming from USB3. Is kinect2_bridge built with CUDA support for depth processing and opencl for registration?

cheers,
Mathieu

[Ross_L]

Hi

Hmm looks like we might be closing in on the issue! It seems that the TX2 does not support OpenCL, so is defaulting to CPU depth registration. I have CUDA set as the depth processing method and I do not see any references or errors to depth method when kinect2_bridge starts, so I am assuming that CUDA is working (this comes pre-installed on the Jetson as you cannot install the CUDA toolkit separetely for some reason).

I checked and have approx_sync set to false for both throttle and rgbd_sync.

Here are the timestamp echos. They seem to line up ok:



Re: the topic names, I have exactly that in my launch file on the jetson:

<remap from="rgbd_image"        to="/rtabmap/rgbd_image"/> <!-- output -->

But still for some weird reason rtabmap doesn't like it. Is this a problem?

Is lack of OpenCL a dealbreaker? I can try switching to the laptop again, but that seemed to suffer from similar problems. That said I could not build CUDA on the laptop due to a bizarre combination of graphics drivers (my laptop has optimus graphics with an old CUDA capable GTX460M) and gcc versions required by CUDA. So the laptop has been using OpenGL and OpenCL.

The laptop also really seems to protest at having three USB items attached even with the hub powered. I've been experimenting with powering the laptop via a boost converter plugged to the lipo (4S, 4500MAh, 40C), to give it power via the DC jack, but the jetson is a much more elegant solution.

Do you know of anyone that has actually successfuly implemented rtabmap on a jetson?

I am starting to feel like the planets are aligned against me haha

[Ross_L]

It seems that the problem may lie within kinect2_bridge. This is what happens when I visualise the raw qhd cloud from kinect2_bridge. You can see a white/grey table leg mapped over the book case behind it:



Does this suggest any possible avenues of troubleshooting? EDIT: This seems to happen when the robot is close to something, inside the depth cameras range. However I have min depth set inside RVIZ as 0.8 m, shouldnt it ignore any clouds under this distance?

[Ross_L]

Quick picture with the table removed:



Looks like the original issue of mismatching is still present :(