RTABmap compiled with OpenCV (CUDA compiled)

[andreacelani]

Hi Mathieu,

I'm working on moving the OpenCV computation to GPU through CUDA 8.0 .
I fixed some issue related to installation of CUDA and compilation of OpenCV (2.4.8).
Now everything is fine except for R200 sensor camera nodelet manager that loaded by the main launcher that dies after the "/camera/driver - Initializing Depth Control Preset to 5"



If I roslaunch 3dsensor_r200.launch the camera driver starts normally and I can see data coming out from "rostopic echo /camera/depth_registered/sw_registered/image_rect_raw"

Seems the nodelet of rtabmap that reads from camera stream stuck the camera driver

here the main launch file portion after the camera loading:

<group ns="rtabmap">
	  
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle" output="screen">
      <param name="rate" type="double" value="10"/>
      <param name="decimation" type="int" value="1"/>

      <remap from="rgb/image_in"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="/camera/depth_registered/sw_registered/image_rect_raw"/>
      <remap from="rgb/camera_info_in" to="/camera/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>
	  
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
	  <param name="database_path"       type="string" value="$(arg database_path)"/>
	  <param name="frame_id"            type="string" value="base_footprint"/>
	  <param name="odom_frame_id"       type="string" value="odom"/>
	  <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
	  <param name="subscribe_depth"     type="bool"   value="true"/>
      <param name="Grid/DepthDecimation"    type="int" value="2"/>
      <param name="Rtabmap/DetectionRate"  type="int" value="1"/>
	  <param name="subscribe_scan"      type="bool"   value="false"/>				<!-- da commentare per eliminare laserscan-->
	  <param name="grid_size"           type="double" value="20"/>  <!-- 4 meters wide -->
	
	  <!-- inputs -->
	  <remap from="scan"            to="/scan"/>				<!-- da commentare per eliminare laserscan-->
	  <remap from="rgb/image"       to="data_throttled_image"/>
  	  <remap from="depth/image"     to="data_throttled_image_depth"/>
  	  <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
  	  
	  <param name="queue_size" type="int" value="100"/>
	  
  	  <!-- output -->
  	  <!-- <remap from="proj_map" to="/map"/> -->
      <remap from="grid_map" to="/map"/>
	
	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
	  <param name="RGBD/ProximityBySpace"        type="string" value="false"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
	  <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
	  <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
	  <param name="Grid/NoiseFilteringRadius" type="string" value="0.1"/>
      <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="5"/>
	  <param name="Reg/Strategy"                 type="string" value="0"/>      <!-- Loop closure transformation refining with ICP: 0=Visual, 1=ICP, 2=Visual+ICP -->
	  <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
	  <param name="Vis/MinInliers"               type="string" value="20"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
	  <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
	  <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
	  <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
	  <param name="Rtabmap/TimeThr"              type="string" value="400"/>
	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
	  <!-- <param name="Optimizer/Slam2D"             type="string" value="true"/>	era true--> <!-- abilita il mapping su superficie piana, diabilita per il 6DOF, era true--> 
	  <param name="Reg/Force3DoF"                type="string" value="true"/>   <!-- abilita il mapping su superficie piana, diabilita per il 6DOF, era true-->    
	  <!-- <param name="Grid/MaxGroundHeight"       type="string" value="0.1"/> -->
      <param name="Grid/MaxGroundAngle"          type="string" value="45"/>
      <param name="Grid/MaxGroundHeight"         type="string" value="0.7"/>       <!-- all points below 20 cm are ground -->
      <param name="Grid/MaxObstacleHeight"       type="string" value="1.7"/>       <!-- all points above 20 cm and below 2 m are obstacles -->
      <param name="Grid/NormalsSegmentation"     type="string" value="false"/> <!-- Use simple passthrough to filter the ground instead of normals segmentation -->
      <param name="Grid/FromDepth"               type="string" value="true"/> <!-- use depth data for map -->
		
	  <!-- localization mode -->
	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
    </node>
   
    <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
    <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="Reg/Force3DoF"               type="string" value="true"/>     <!-- abilita il mapping su superficie piana, disabilita per il 6DOF, era true-->
      <param name="Vis/InlierDistance"          type="string" value="0.1"/>
      <param name="Odom/ResetCountdown"         type="string" value="10" />     <!-- reset odometry when lost 10 frames -->
		
      <remap from="rgb/image"       to="data_throttled_image"/>
      <remap from="depth/image"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
    </node>
    
    <!-- visualization with rtabmapviz -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth"             type="bool" value="true"/>
      <param name="subscribe_scan"              type="bool" value="true"/>				<!-- da commentare per eliminare laserscan-->
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
    
      <remap from="rgb/image"       to="data_throttled_image"/>
      <remap from="depth/image"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="data_throttled_camera_info"/>
      <remap from="scan"            to="/scan"/>				<!-- da commentare per eliminare laserscan-->
    </node>
    
  </group>

  <!-- point cloud generation for obstacles detection -->
  <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz pcl_manager">
      <remap from="depth/image"            to="/rtabmap/data_throttled_image_depth"/>
      <remap from="depth/camera_info"      to="/rtabmap/data_throttled_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="2"/>
      <param name="max_depth"  type="double" value="4.0"/>
	  <param name="min_depth"  type="double" value="1.5"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>

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

      <param name="frame_id"             type="string" value="base_footprint"/>
	  <param name="Grid/NormalsSegmentation" type="string" value="false"/> 
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="Grid/MaxGroundHeight"         type="string" value="0.4"/>       <!-- was 0.7 all points below 20 cm are ground -->
      <param name="Grid/MaxGroundAngle"  type="double" value="0.785"/>
      <param name="Grid/MinClusterSize"     type="int" value="50"/>
      <param name="Grid/MaxObstacleHeight" type="double" value="1.0"/>       <!-- was 1.5 -->
      <param name="queue_size"           type="int" value="100"/>
    </node>  

Any idea how the OpenCV-Cuda_compiled has affected the reading of camera stream?
Thank you!

Andrea

[andreacelani]

I tried to unistall CUDA 8.0 and then compile OpenCV without CUDA (-D WITH_CUDA=OFF).
After that I compiled RTABmap --> ok and I compiled RTABmap-ros--> errors.

Then I compiled the OpenCV without CUDA and without OPENCL.
Both compilation of RTABmap and RTABmap-ros was ok.

Now the camera nodelet manager starts fine and the camera (R200) is running, streaming data to RTABmap and RVIZ but the pcl-manager used to create cloud points from depth and color streams died.

Before of that everything was fully working without OpenCV and CUDA

Any idea how camera manager and pcl-manager could be affected by OpenCV (CUDA)?

Thank you

Andrea

[andreacelani]

ok, I installed again CUDA 8.0 and I compiled OpenCV 2.4.8 with CUDA and wituout OPENCL (WITH_OpenCL=OFF):

cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D ENABLE_FAST_MATH=1 -D CUDA_FAST_MATH=1 -D WITH_CUBLAS=1 -D CUDA_GENERATION=Kepler -D WITH_OPENCL=OFF ..

I set rtabmap to work with GPU SIFT:

<param name="Kp/DetectorStrategy"          type="string" value="0"/>  <!--[0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.]-->
	  <param name="SURF/Extended"                type="bool" value="false"/>  <!--[Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).]-->
	  <param name="SURF/GpuKeypointsRatio"       type="double" value="0.01"/>  <!--[Used with SURF GPU.]-->
	  <param name="SURF/GpuVersion"              type="bool" value="false"/>  <!--[GPU-SURF: Use GPU version of SURF. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.]-->
	  <param name="SURF/HessianThreshold"        type="int" value="500"/>  <!--[Threshold for hessian keypoint detector used in SURF.]-->
	  <param name="SURF/OctaveLayers"            type="int" value="2"/>  <!--[Number of octave layers within each octave.]-->
	  <param name="SURF/Octaves"                 type="int" value="4"/>  <!--[Number of pyramid octaves the keypoint detector will use.]-->
	  <param name="SURF/Upright"                 type="bool" value="false"/>  <!--[Up-right or rotated features flag (true - do not compute orientation of features; false - compute orientation).]-->
	  <param name="Vis/FeatureType"              type="int" value="6"/>  <!--[0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.]-->

Now the R200 works fine and camera nodelet manager works fine as well.
But the PCL manager dies with the following error:

terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2017-07-22 17:17:51.623) util3d_filtering.cpp:145::voxelize() Condition ((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())) not met!

I'll try changing parameters compilation of OpenCV 2.4.8. I hope to find a setting (without OpenGL or something else) that fix the error as per camera nodelet manager happened.

Any suggestion is welcome

[matlabbe]

What do you mean by "PCL manager"? This is maybe point_cloud_xyzrgb that is asserting. Make sure to update latest rtabmap_ros code too.

[andreacelani]

Yes I meant point_cloud_xyzrgb.
By the way you fixed my issue: I worked with an updated version of RTABmap and an old version of RTABmap-ros. My mistake. Thank you!!
If someone is interested I also found the correct cmake setting of OpenCV 2.4.8 building with CUDA 8.0 on GTX1050 (Compute Capability:6.1) :

cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D ENABLE_FAST_MATH=1 -D CUDA_FAST_MATH=1 -D WITH_CUBLAS=1 -D CUDA_ARCH_BIN=6.1 -D CUDA_ARCH_PTX="" -D WITH_OPENCL=ON ..

Thank you again.

Andrea

[funnyrabbit]

Hi Andrea,

I'm now working with RTAB-Map and prefer to use CUDA for OpenCV building map because my CPU on full loading at all. Could you show me how to do it? I really appreciated that!
Thank you very much Andrea!

Regards,
Rabbit.