search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

rtabmap_slam crashes when using Superglue and Superpoint #977

Closed GVMafeedme closed 1 year ago

GVMafeedme commented 1 year ago

I am testing Superglue with Superpoint, I built rtabmap from source, but when I set the parameters to use Superpoint and Superglue, it crashes instantly. I am using docker with access to gpu (gazebo launches correctly) and has built rtabmap bot for gpu and cpu, here is the Dockerfile for cpu:

FROM osrf/ros:noetic-desktop-full
SHELL ["/bin/bash", "-c"]

# ROS INSTALL 
RUN apt-get update && apt-get install apt-utils -y
RUN apt-get install wget git ros-noetic-catkin python3-catkin-tools python-is-python3 python3-pip curl -y

# RTABMAP INSTALL
RUN apt-get install  libopencv-dev python3-opencv ros-noetic-vision-opencv -y

# TUTORIAL PACKAGES INSTALL
RUN apt-get install ros-noetic-grid-map -y

# rebuild rtabmap with torch
RUN apt-get install ros-noetic-rtabmap ros-noetic-rtabmap-ros -y
RUN apt-get remove  ros-noetic-rtabmap ros-noetic-rtabmap-ros -y

RUN apt-get install software-properties-common -y
RUN rm -rf /var/lib/apt/lists/*
RUN add-apt-repository ppa:borglab/gtsam-release-4.0
RUN apt-get update && apt-get install libgtsam-dev libgtsam-unstable-dev -y

WORKDIR /torch
RUN wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
RUN apt-get install unzip -y
RUN unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip

WORKDIR /
RUN git clone -b 0.21.0 https://github.com/introlab/rtabmap.git rtabmap
WORKDIR /rtabmap/build
RUN export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/torch/libtorch/lib && \
    CXXFLAGS="-I/usr/include/python3.8/" \
    cmake -D WITH_TORCH=ON \
          -D WITH_PYTHON=ON \
          -D Torch_DIR=/torch/libtorch/share/cmake/Torch \
        .. && make -j8 && make install

WORKDIR /ros_ws
RUN git clone --branch noetic-devel https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
RUN cp -r /torch/libtorch/lib/ /usr/local/lib/libtorch
RUN source devel/setup.bash && \
    export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/torch/libtorch/lib && \
    catkin_make -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON

ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/torch/libtorch/lib:/usr/local/lib/

WORKDIR /torch/superpoint
RUN wget https://raw.githubusercontent.com/KinglittleQ/SuperPoint_SLAM/master/superpoint.pt

CMD [ "bash" ]

The launch group I used is this one:

<group ns="rtabmap">
    <node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
      <remap from="rgb/image"         to="/camera/color/image_raw"/>
      <remap from="depth/image"       to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="true"/>
      <!-- <param unless="$(eval model=='waffle')" name="subscribe_rgbd" type="bool" value="false"/> -->
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="approx_sync"         type="bool"   value="true"/>

      <!-- use actionlib to send goals to move_base -->
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

      <!-- inputs -->
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/odom"/>
      <remap from="rgbd_image"       to="rgbd_image"/>

      <!-- output -->
      <remap from="grid_map" to="/map"/>

      <!-- RTAB-Map's parameters -->
      <param name="Vis/EstimationType" type="string" value="1"/>
      <param name="Vis/MinInliers" type="string" value="20"/>

      <param name="Kp/DetectorStrategy"    type="string" value="11"/>
      <param name="Vis/FeatureType" type="string" value="11"/>
      <param name="SuperPoint/Cuda"        type="string" value="false"/>
      <param name="SuperPoint/ModelPath" type="string" value="/torch/superpoint/superpoint.pt"/>

      <param name="Vis/CorNNType"      type="string" value="6"/>
      <param name="PyMatcher/Model" type="string" value="indoor"/>
      <param name="PyMatcher/Path" type="string" value="/rtabmap/corelib/src/pymatcher/rtabmap_superglue.py"/>
      <param name="PyMatcher/Threshold" type="string" value="0.2"/>
      <param name="PyMatcher/Iterations" type="string" value="20"/>
      <param name="PyMatcher/Cuda" type="string" value="false"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
    </node>

    <!-- visualization with rtabmap_viz -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_scan"   type="bool"   value="true"/>
      <param name="subscribe_odom"   type="bool"   value="true"/>
      <param name="frame_id"         type="string" value="base_link"/>
      <param name="approx_sync"      type="bool"   value="true"/>

      <remap from="odom"            to="/odom"/>
      <remap from="scan"            to="/scan"/>
    </node>
  </group>

The output is the following, not much information is provided by --udebug either:

[ INFO] [1685614027.698404525]: /rtabmap/rgbd_sync: approx_sync = true
[ INFO] [1685614027.698513670]: /rtabmap/rgbd_sync: approx_sync_max_interval = 0.000000
[ INFO] [1685614027.698557622]: /rtabmap/rgbd_sync: queue_size  = 10
[ INFO] [1685614027.698596370]: /rtabmap/rgbd_sync: depth_scale = 1.000000
[ INFO] [1685614027.698654789]: /rtabmap/rgbd_sync: decimation = 1
[ INFO] [1685614027.698686284]: /rtabmap/rgbd_sync: compressed_rate = 0.000000
[ INFO] [1685614027.722617701]: 
/rtabmap/rgbd_sync subscribed to (approx sync):
   /camera/color/image_raw \
   /camera/depth/image_raw \
   /camera/color/camera_info
[ INFO] [1685614027.722755963]: Initializing nodelet with 20 worker threads.
[ INFO] [1685614027.858074865]: Starting node...
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-germanvega'
[ INFO] [1685614027.940920083]: rtabmap_viz: Using configuration from "/ros_ws/src/rtabmap_ros/rtabmap_demos/launch/config/rgbd_gui.ini"
[ INFO] [1685614028.070844790]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1685614028.071130109]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1685614028.071153110]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1685614028.071203494]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1685614028.071225869]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1685614028.071236979]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1685614028.071248269]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1685614028.071259316]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1685614028.071530482]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1685614028.087200043]: rtabmap: frame_id      = base_link
[ INFO] [1685614028.087390931]: rtabmap: map_frame_id  = map
[ INFO] [1685614028.087452245]: rtabmap: log_to_rosout_level = 4
[ INFO] [1685614028.087473392]: rtabmap: initial_pose  = 
[ INFO] [1685614028.087489237]: rtabmap: use_action_for_goal  = true
[ INFO] [1685614028.087554186]: rtabmap: tf_delay      = 0.050000
[ INFO] [1685614028.087574502]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1685614028.087593869]: rtabmap: odom_sensor_sync   = false
[ INFO] [1685614028.088213789]: rtabmap: gen_scan  = false
[ INFO] [1685614028.088303918]: rtabmap: gen_depth  = false
[ INFO] [1685614028.161390298]: Setting RTAB-Map parameter "GridGlobal/MinSize"="20"
[ INFO] [1685614028.185811936]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="11"
[ INFO] [1685614028.206411181]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1685614028.243912164]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1685614028.252144212]: Physics dynamic reconfigure ready.
[ INFO] [1685614028.283411281]: Setting RTAB-Map parameter "PyMatcher/Cuda"="false"
[ INFO] [1685614028.284704522]: Setting RTAB-Map parameter "PyMatcher/Iterations"="20"
[ INFO] [1685614028.285326070]: Setting RTAB-Map parameter "PyMatcher/Model"="indoor"
[ INFO] [1685614028.286190287]: Setting RTAB-Map parameter "PyMatcher/Path"="/rtabmap/corelib/src/pymatcher/rtabmap_superglue.py"
[ INFO] [1685614028.286690300]: Setting RTAB-Map parameter "PyMatcher/Threshold"="0.2"
[ INFO] [1685614028.326689741]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1685614028.328213817]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[INFO] [1685614028.356244, 0.000000]: wait_for_service(/gazebo/spawn_urdf_model): finally were able to contact [rosrpc://0232f45435f4:59469]
[INFO] [1685614028.358447, 0.000000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1685614028.388100920]: Setting RTAB-Map parameter "SuperPoint/Cuda"="false"
[ INFO] [1685614028.388559412]: Setting RTAB-Map parameter "SuperPoint/ModelPath"="/torch/superpoint/superpoint.pt"
[ INFO] [1685614028.396551916]: Setting RTAB-Map parameter "Vis/CorNNType"="6"
[ INFO] [1685614028.399341242]: Setting RTAB-Map parameter "Vis/EstimationType"="1"
[ INFO] [1685614028.399577175]: Setting RTAB-Map parameter "Vis/FeatureType"="11"
[ INFO] [1685614028.405441169]: Setting RTAB-Map parameter "Vis/MinInliers"="20"
[ WARN] [1685614028.497210692, 2919.000000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist. canTransform: source_frame base_link does not exist.. canTransform returned after 2919 timeout was 0.1.
../src/intel/isl/isl.c:2105: FINISHME: ../src/intel/isl/isl.c:isl_surf_supports_ccs: CCS for 3D textures is disabled, but a workaround is available.
[ WARN] [1685614028.510016367]: Setting "Grid/Sensor" parameter to 0 (default 1) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from camera's cloud projection, set "Grid/Sensor" to 1. To suppress this warning, add <param name="Grid/Sensor" type="string" value="0"/>
[ INFO] [1685614028.510374299]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true and Grid/Sensor is 0.
[ WARN] [1685614028.510503049]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 10 (default 0) as "subscribe_scan" is true and "Reg/Strategy" uses ICP. Proximity detection by space will be also done by merging close scans. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
[ INFO] [1685614028.512788380]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1685614028.513501816]: rtabmap: Using database from "/home/germanvega/.ros/rtabmap.db" (0 MB).
[DEBUG] (2023-06-01 10:07:08.519) OctoMap.cpp:338::OctoMap() fullUpdate_         =true
[DEBUG] (2023-06-01 10:07:08.519) OctoMap.cpp:339::OctoMap() updateError_        =0.010000
[DEBUG] (2023-06-01 10:07:08.519) OctoMap.cpp:340::OctoMap() rangeMax_           =0.000000
[DEBUG] (2023-06-01 10:07:08.519) OctoMap.cpp:341::OctoMap() rayTracing_         =false
[DEBUG] (2023-06-01 10:07:08.519) OctoMap.cpp:342::OctoMap() emptyFloodFillDepth_=0
[DEBUG] (2023-06-01 10:07:08.519) Rtabmap.cpp:317::init() path=/home/germanvega/.ros/rtabmap.db
[ INFO] (2023-06-01 10:07:08.519) Rtabmap.cpp:322::init() Using database "/home/germanvega/.ros/rtabmap.db".
[DEBUG] (2023-06-01 10:07:08.520) Rtabmap.cpp:4552::setWorkingDirectory() Comparing new working directory path "/home/germanvega/.ros" with ""
[DEBUG] (2023-06-01 10:07:08.520) Rtabmap.cpp:289::setupLogFiles() Log disabled!
[DEBUG] (2023-06-01 10:07:08.520) SuperPoint.cc:122::SPDetector() modelPath=/torch/superpoint/superpoint.pt thr=0.010000 nms=1 cuda=0
[ INFO] [1685614028.577616840]: Parameters are not saved! (No configuration file provided...)
rtabmap: Saving database/long-term memory... (located at /home/germanvega/.ros/rtabmap.db)
[ INFO] (2023-06-01 10:07:08.577) Rtabmap.cpp:460::close() databaseSaved=1
[ INFO] (2023-06-01 10:07:08.577) Rtabmap.cpp:6190::clearPath() status=0
[DEBUG] (2023-06-01 10:07:08.578) Rtabmap.cpp:549::parseParameters() 
[DEBUG] (2023-06-01 10:07:08.578) Rtabmap.cpp:289::setupLogFiles() Log disabled!
[DEBUG] (2023-06-01 10:07:08.578) Rtabmap.cpp:678::parseParameters() new detector strategy 2
[DEBUG] (2023-06-01 10:07:08.578) BayesFilter.cpp:114::setPredictionLC() predictionEpsilon = 0.000000
[DEBUG] (2023-06-01 10:07:08.578) BayesFilter.cpp:114::setPredictionLC() predictionEpsilon = 0.000000
rtabmap: Saving database/long-term memory...done! (located at /home/germanvega/.ros/rtabmap.db, 0 MB)
../src/intel/isl/isl.c:2105: FINISHME: ../src/intel/isl/isl.c:isl_surf_supports_ccs: CCS for 3D textures is disabled, but a workaround is available.
[rtabmap/rtabmap-7] process has died [pid 4168, exit code -11, cmd /ros_ws/devel/lib/rtabmap_slam/rtabmap -d --udebug move_base:=/move_base scan:=/scan odom:=/odom rgbd_image:=rgbd_image grid_map:=/map __name:=rtabmap __log:=/home/germanvega/.ros/log/04a71064-0063-11ee-b0d6-0242ac120002/rtabmap-rtabmap-7.log].
log file: /home/germanvega/.ros/log/04a71064-0063-11ee-b0d6-0242ac120002/rtabmap-rtabmap-7*.log

I tried with version 0.21.1 but it does crash instantly as well. That log file referenced in the end does not exist in my computer.

matlabbe commented 1 year ago

A while ago I did try it with docker and it worked in ros (using D435i camera live). See https://github.com/introlab/rtabmap/issues/896#issuecomment-1257085425 Never tried it with torch using only CPU. Did you try starting from pytorch CPU docker image instead of the ros one?

GVMafeedme commented 1 year ago

Thank you! I found the issue: the weights are compatible with a few versions of torch only, so all the weights should be updated, you gave the answer here https://github.com/introlab/rtabmap_ros/issues/957

matlabbe commented 6 months ago

(Adding this note here for reference) There is a superpoint/superglue rtabmap_ros docker image here for convenience: https://github.com/introlab/rtabmap_ros/blob/master/docker/noetic/superpoint/Dockerfile