search

MarvinStuede / cmr_lidarloop

Method to add Lidar based loop detections to Graph based SLAM
BSD 3-Clause "New" or "Revised" License
17 stars 4 forks source link

RTAB-Map + cmr_lidar_loop not working for mapping. #4

Open ahar opened 3 years ago

ahar commented 3 years ago

Hi,

I am trying to use only a 3D Lidar ( velodyne) for loop closure detection and map update using RTAB-MAP. cmr_lidarloop while interfaced with RTAB-Map in mapping mode expects the mapData to be loaded. Is that correct? If I don't provide the database as an input, which is the case for mapping, the lidar_loopdetrection.cpp does not come out of while(true). It means that the following lines including the declaring the publisher, and subscribers don't execute. Additionally, I am finding the following warnings.

[ WARN] [1627537412.899023582, 227.069921775]: Detected jump back in time of 0.00757823s. Clearing TF buffer. [ WARN] [1627537413.183986347, 227.353581530]: Detected jump back in time of 0.00891847s. Clearing TF buffer. [ WARN] [1627537414.338962861, 228.517350990]: Detected jump back in time of 0.00014901s. Clearing TF buffer. [ WARN] [1627537414.399023846, 228.568079650]: Detected jump back in time of 0.00942035s. Clearing TF buffer. [ WARN] [1627537414.459000417, 228.628807720]: Detected jump back in time of 0.00869228s. Clearing TF buffer. [ WARN] [1627537414.999089753, 229.175264124]: Detected jump back in time of 0.00223588s. Clearing TF buffer. [ WARN] [1627537415.927885781, 230.106547676]: /rtabmap/rtabmap_mapping: 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_mapping subscribed to (approx sync): /odom \ /scan_descriptor

The launch file used for RTAB-Map mapping is given below. Also, I have recorded a Bag file that contains the velodyne data, tfand wheel odometry. I am supplying it after executing cmr_localization/cmr_localization/launch/mapping.launch in a separate shell.

`<?xml version="1.0"?>

`

MarvinStuede commented 2 years ago

Hi,

sorry for the late reply, I hope the question is still relevant. I don't know if I get it correctly, but are you using the cmr_localization/cmr_localization/launch/mapping.launch or the file you provided (which is incomplete I suppose). Because the mapping.launch file is a custom file for the Sobi robot with two RGBD cameras and a Lidar.

cmr_lidarloop while interfaced with RTAB-Map in mapping mode expects the mapData to be loaded. Is that correct? If I don't provide the database as an input, which is the case for mapping, the lidar_loopdetrection.cpp does not come out of while(true).

You would still provide a path to a database as an input to RTABMap. This database will then be created at startup. If you set the localization parameter of the rtabmap_ros.launch file to false, RTABMAP will set thertabmap/Mem/IncrementalMemory parameter to true, which we use to check if mapping mode is active. cmr_lidarloop should then leave the while loop you mentioned.

ahar commented 2 years ago

Hi Marvin, The following is the launch file that I am using. I want to use both the RGBD images and LIDAR data to be used by rtabmap with cmr_lidarloop setting.

<launch>

  <!-- Choose visualization -->
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="false" /> 
  <arg name="subscribe_scan_descriptor" default="true"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_descriptor_topic"   default="scan_descriptor"/>

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

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

  <arg name="use_2d_laser" default="false" />
  <arg name="queue_size"              default="20"/>

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

  <group ns="rtabmap">

    <!-- SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" clear_params="true">

      <param name="database_path" type="string" value="$(arg database_path)"/>

      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="frame_id"           type="string" value="base_link"/>

      <!-- Which Sensor combination to use -->
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_rgb"   type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="1"/>
      <param name="approx_sync"          type="bool" value="true"/>
      <param name="wait_for_transform" type="bool" value="true"/>

      <remap from="/rtabmap/scan_descriptor"        to="/cmr_lidarloop/$(arg scan_descriptor_topic)"/>
      <remap from="rgbd_image"       to="/camera1/rgbd_image/compressed"/>
      <!-- <remap from="rgbd_image1"       to="/camera2/rgbd_image/compressed"/> -->
      <!--remap from="rgbd_image"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/-->
      <remap from="scan_cloud"        to="/velodyne_points"/>
      <remap from="initialpose" to="/initialpose"/>

      <param name="Rtabmap/DetectionRate" type="string" value="3"/>

      <param name="Rtabmap/MemoryThr" type="int" value="0"/>
      <param name="Rtabmap/TimeThr" type="string" value="0"/>
      <param name="Mem/ReduceGraph" type="bool" value="false"/>  

      <!-- should be 0 for multi-cameras -->
      <param name="Vis/EstimationType" type="string" value="1"/> 
      <param name="Vis/CorGuessWinSize" type="string" value="0"/>
      <param name="Vis/MinInliers" type="string" value="15"/>

      <param name="odom_frame_id"            type="string" value="/odom"/>
      <param name="odom_tf_linear_variance"  value="0.005"/>
      <param name="odom_tf_angular_variance" value="0.005"/>      

      <!-- RTAB-Map Output -->
      <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/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
      <param name="RGBD/ProximityByTime"      type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!-- Do also proximity detection by space by merging close scans together. -->
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
      <param name="RGBD/OptimizeMaxError"     type="string" value="0"/>     <!-- Reject any loop closure causing large errors (>3x link's covariance) in the map -->
      <param name="RGBD/LoopClosureReextractFeatures"     type="bool" value="false"/>
      <param name="RGBD/LocalBundleOnLoopClosure"         type="bool" value="true"/>
      <param name="RGBD/ProximityOdomGuess"               type="bool" value="false"/>

      <param name="Optimizer/Robust"          type="string" value="false"/>   

      <!-- Create 2D occupancy grid from laser scan -->
      <param name="Grid/FromDepth"            type="string" value="false"/> 
      <param name="Grid/RangeMax"             type="string" value="20"/>      
      <param name="Grid/NormalsSegmentation"  type="string" value="false"/>     

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

      <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <!--param name="Reg/Strategy"              type="string" value="2"/-->     
      <!-- <param if="$(arg use_2d_laser)"     name="Reg/Strategy" type="string" value="2"/>
      <param unless="$(arg use_2d_laser)" name="Reg/Strategy" type="string" value="0"/> -->
      <param name="Reg/Strategy"              type="string" value="2"/>

      <!-- 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="Vis/FeatureType"        type="string" value="0"/>   
      <!-- 3D visual words correspondence distance -->
      <param name="Vis/MaxDepth"          type="string" value="15.0"/>  
      <param name="Vis/DepthAsMask"          type="string" value="false"/>  

      <!-- 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="0"/>
      <!--param name="Kp/RoiRatios" type="string" value="0 0 0 0.5"/-->

      <!--Extract more SURF features-->
      <param name="SURF/HessianThreshold" type="string" value="60"/>    

      <param name="Mem/DepthAsMask" type="string" value="false"/>

       <!-- Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres -->
      <param name="Optimizer/Strategy"          type="int" value="2"/>
      <param name="Optimizer/Iterations"          type="int" value="20"/>
      <param name="Optimizer/Epsilon"          type="string" value="0.00001"/>
      <param name="Optimizer/Robust"          type="string" value="false"/>

       <!-- ICP parameters for velodyne data-->
       <param name="Icp/VoxelSize"                  type="string" value="0.2"/>
       <param name="Icp/PointToPlaneK"              type="string" value="20"/>
       <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
       <param name="Icp/PointToPlane"               type="string" value="true"/>
       <param name="Icp/Iterations"                 type="string" value="30"/>
       <param name="Icp/Epsilon"                    type="string" value="0.00001"/>
       <param name="Icp/MaxTranslation"             type="string" value="3"/>
       <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
       <param name="Icp/PM"                         type="string" value="true"/> 
       <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
       <param name="Icp/CorrespondenceRatio"        type="string" value="0.1"/>

      <!-- DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11 -->
      <!--param     if="$(arg localization)" name="RGBD/MarkerDetection" type="string" value="false"/>
      <param unless="$(arg localization)" name="RGBD/MarkerDetection" type="string" value="true"/>

      <param name="Marker/Dictionary"           type="string" value="10"/>
      <param name="Marker/Length"               type="string" value="0.398"/>
      <param name="Marker/VarianceAngular"      type="string" value="9999"/-->           

      <!-- 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>

    <!-- Visualisation RTAB-Map -->
    <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="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="1"/>
      <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>

      <remap from="rgbd_image0"       to="/camera1/rgbd_image/compressed"/>
      <!-- <remap from="rgbd_image1"       to="/camera2/rgbd_image/compressed"/> -->

      <param name="subscribe_scan"     type="bool" value="$(arg use_2d_laser)"/>
      <param name="frame_id"           type="string" value="base_link"/>
      <param name="wait_for_transform" type="bool" value="true"/>

      <remap from="scan"            to="/r2000_driver_node/scan"/>
      <remap from="odom"            to="/odom"/>
      <remap from="/rtabmap/scan_descriptor"        to="/cmr_lidarloop/$(arg scan_descriptor_topic)"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>

  </group>

</launch>

The following error pops up as soon as I play this launch file along with save_detector_data.launch.

[FATAL] [1638968435.476764729]: ASSERTION FAILED
        file = /home/drive/cmr_ws/src/cmr_lidarloop/src/save_data_for_detector.cpp
        line = 136
        cond = s.globalDescriptors().size() == 1

I am yet to figure out why global_descriptor size becomes zero. BTW, I am using rtabmap-0.20.7 in ubuntu-18.04. Can you suggest a route to resolve this?

thank you,

MarvinStuede commented 2 years ago

What exactly is your use case? Are you trying to train a new detector with your own Lidar data? If you only want to do mapping/localization, you could try to use the pre-trained detector from the repository first, by just launching cmr_lidarloop.launch. Did you also configure the scan topic in https://github.com/MarvinStuede/cmr_lidarloop/blob/master/cfg/cmr_lidarloop_params.yaml ?

ahar commented 2 years ago

I am trying to train a new detector from my recorded data. The data contains both RGBD images and a 3D LIDAR data. scan topic is also configured properly, as given in the above launch file i.e., /velodyne_points <remap from="scan_cloud" to="/velodyne_points"/>

MarvinStuede commented 2 years ago

Are the scan_descriptor topics connected to both nodes? cmr_lidarloop uses the /cmr_lidarloop/scan_descriptor. Topic https://github.com/MarvinStuede/cmr_lidarloop/blob/ff9493b0cb1c54ce6a3dfa82c3e98fcc38420d34/src/save_data_for_detector.cpp#L194 It seems that you are using /rtabmap/scan_descriptortopic instead.

ahar commented 2 years ago

you are right. I am using /rtabmap/scan_descriptor. Otherwise, rtabmap was unable to find scan_descriptor data. Let me try once with this setting.

ahar commented 2 years ago

Hi Marvin,

I checked again. I followed your latest commit. In accordance with your latest commit, I added <remap from="/rtabmap/scan_descriptor" to="/cmr_lidarloop/scan_descriptor"/> in my rtabmap mapping launch file. However, I received the same error again. The map_data is received once and then it throws an error. The log is given below.

[ INFO] [1639885149.204053458]: The cmr_lidarloop parameters were read successfully.
[ INFO] [1639885149.204483489]: Please start RTAB-Map.
[ INFO] [1639885151.232232993]: Subscribed to /rtabmap/mapData and /rtabmap/info
[ INFO] [1639885155.089717712, 1637764111.208592639]: Received map data!
[FATAL] [1639885155.096176889, 1637764111.218665304]: ASSERTION FAILED
    file = /home/anindya/novus_ws/src/cmr_lidarloop/src/save_data_for_detector.cpp
    line = 135
    cond = s.globalDescriptors().size() == 1

[cmr_lidarloop/save_data_for_detector-1] process has died [pid 139568, exit code -5, cmd /home/anindya/novus_ws/devel/lib/cmr_lidarloop/save_data_for_detector __name:=save_data_for_detector __log:=/home/anindya/.ros/log/a657e434-606d-11ec-b475-4d9491e59340/cmr_lidarloop-save_data_for_detector-1.log].
log file: /home/anindya/.ros/log/a657e434-606d-11ec-b475-4d9491e59340/cmr_lidarloop-save_data_for_detector-1*.log
ahar commented 2 years ago

Hi Marvin,

I also tried with your pretrained model, i.e., using the raw detector data and the .pickle file from the git repo. However, I find the following warnings in the shell running cmr_lidarloop.launch 

[ WARN] (2021-12-20 11:55:15.157) SensorData.cpp:718::uncompressDataConst() Requested laser scan data, but the sensor data (35) doesn't have laser scan.
[ WARN] [1639981515.157458536, 1636958965.997447325]: Map descriptor wrong size

In the shell of my rtabmap.launch I find the following error. [ERROR] (2021-12-20 09:16:00.312) RegistrationIcp.cpp:1473::computeTransformationImpl() Laser scans empty?!? (new[151]=0 old[150]=0)

P.S. I have edited the above post (of 11 days ago) and the rtabmap launch file used for the two latest cases (while preparing my detector dataset and with your model ) is provided there.

regards,