Dense map

.gitignore

@@ -1,4 +1,6 @@
 CMakeLists.txt.user
+.idea/
+cmake-build-debug
 KeyFrameTrajectory.txt
 Thirdparty/DBoW2/build/
 Thirdparty/DBoW2/lib/

CMakeLists.txt

@@ -5,40 +5,58 @@ IF(NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE Release)
 ENDIF()
 
-#MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
-
 # Check for c++11 support
 INCLUDE(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-IF(COMPILER_SUPPORTS_CXX11)
-  add_compile_options(-std=c++11)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+IF(COMPILER_SUPPORTS_CXX14)
+  #add_compile_options(-std=c++14)
+  set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS}")
 ELSE()
-  MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+  MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
 ENDIF()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/orb_slam2/cmake_modules)
 
 find_package (catkin REQUIRED COMPONENTS
 roscpp
+rospy
 std_msgs
 cv_bridge
 image_transport
 tf
+tf2_geometry_msgs
+tf2_ros
 sensor_msgs
 pcl_conversions
 pcl_ros
+dynamic_reconfigure
+message_generation
 )
 
-find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4.0 QUIET)
 if(NOT OpenCV_FOUND)
-   find_package(OpenCV 2.4.3 QUIET)
+   find_package(OpenCV 3.0 QUIET)
    if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+      find_package(OpenCV 2.4.3 QUIET)
+      if(NOT OpenCV_FOUND)
+         message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+     endif()
    endif()
 endif()
 
 find_package(Eigen3 3.1.0 REQUIRED)
 
+set (DYNAMIC_RECONFIGURE_PATH ros/config/dynamic_reconfigure.cfg)
+execute_process(COMMAND chmod a+x ${DYNAMIC_RECONFIGURE_PATH}
+        WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+        RESULT_VARIABLE cmd_result
+        OUTPUT_VARIABLE cmd_ver)
+message(STATUS "Chmod a+x the dynamic_reconfigure file")
+
+generate_dynamic_reconfigure_options(
+        ${DYNAMIC_RECONFIGURE_PATH}
+)
+
 set(LIBS_ORBSLAM
 ${OpenCV_LIBS}
 ${EIGEN3_LIBS}
@@ -53,8 +71,18 @@ ${OpenCV_LIBS}
 ${catkin_LIBRARIES}
 )
 
+add_service_files(
+        FILES
+        SaveMap.srv
+)
+
+generate_messages(
+        DEPENDENCIES
+        std_msgs
+)
+
 catkin_package (
-  CATKIN_DEPENDS roscpp std_msgs cv_bridge image_transport tf sensor_msgs
+  CATKIN_DEPENDS roscpp rospy std_msgs cv_bridge image_transport tf sensor_msgs dynamic_reconfigure message_runtime
   LIBRARIES {PROJECT_NAME} libDBoW2 libg2o libmt_task_queue
 )
 
@@ -98,11 +126,19 @@ target_link_libraries(${PROJECT_NAME}
 ${LIBS_ORBSLAM}
 )
 
+# map serialization addition - library boost serialization 
+message(STATUS "Compile With map save/load function")
+find_library(BOOST_SERIALIZATION boost_serialization)
+if (NOT BOOST_SERIALIZATION)
+    message(FATAL_ERROR "Can't find libboost_serialization")
+endif()
+target_link_libraries(${PROJECT_NAME} ${BOOST_SERIALIZATION})
+
 add_executable (${PROJECT_NAME}_mono
 ros/src/MonoNode.cc
 ros/src/Node.cc
 )
-add_dependencies (${PROJECT_NAME}_mono ${PROJECT_NAME})
+add_dependencies (${PROJECT_NAME}_mono ${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 target_link_libraries(${PROJECT_NAME}_mono
 ${LIBS_ROS}
@@ -112,7 +148,7 @@ add_executable (${PROJECT_NAME}_stereo
 ros/src/StereoNode.cc
 ros/src/Node.cc
 )
-add_dependencies (${PROJECT_NAME}_stereo ${PROJECT_NAME})
+add_dependencies (${PROJECT_NAME}_stereo ${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 target_link_libraries(${PROJECT_NAME}_stereo
 ${LIBS_ROS}
@@ -122,7 +158,7 @@ add_executable (${PROJECT_NAME}_rgbd
 ros/src/RGBDNode.cc
 ros/src/Node.cc
 )
-add_dependencies (${PROJECT_NAME}_rgbd ${PROJECT_NAME})
+add_dependencies (${PROJECT_NAME}_rgbd ${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 target_link_libraries(${PROJECT_NAME}_rgbd
 ${LIBS_ROS}
@@ -144,6 +180,10 @@ install(DIRECTORY ros/launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch
 )
 
+install(DIRECTORY ros/config/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/config
+)
+
 install(DIRECTORY orb_slam2/config/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/orb_slam2/config
 )

README.md

@@ -3,12 +3,16 @@
 The original implementation can be found [here](https://github.com/raulmur/ORB_SLAM2.git).
 
 # ORB-SLAM2 ROS node
-This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Monocular**, **Stereo** and **RGB-D** cameras that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale). It is able to detect loops and relocalize the camera in real time. This implementation removes the Pangolin dependency, and the original viewer. All data I/O is handled via ROS topics. For vizualization you can use RViz. This repository is maintained by [Lennart Haller](http://lennarthaller.de) on behalf of [appliedAI](http://appliedai.de).
+This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Monocular**, **Stereo** and **RGB-D** cameras that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale). It is able to detect loops and relocalize the camera in real time. This implementation removes the Pangolin dependency, and the original viewer. All data I/O is handled via ROS topics. For visualization you can use RViz. This repository is maintained by [Lennart Haller](http://lennarthaller.de) on behalf of [appliedAI](http://appliedai.de).
 ## Features
 - Full ROS compatibility
+- Supports a lot of cameras out of the box, such as the Intel RealSense family. See the run section for a list
 - Data I/O via ROS topics
-- Parameters can be set via the ROS parameters server during runtime
+- Parameters can be set with the rqt_reconfigure gui during runtime
 - Very quick startup through considerably sped up vocab file loading
+- Full Map save and load functionality based on [this PR](https://github.com/raulmur/ORB_SLAM2/pull/381).
+- Loading of all parameters via launch file
+- Supports loading cam parameters from cam_info topic
 
 ### Related Publications:
 [Monocular] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. (**2015 IEEE Transactions on Robotics Best Paper Award**). **[PDF](http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf)**.
@@ -48,83 +52,159 @@ if you use ORB-SLAM2 (Stereo or RGB-D) in an academic work, please cite:
       year={2017}
      }
 
-# 2. Prerequisites
+# 2. Building orb_slam2_ros
 We have tested the library in **Ubuntu 16.04** with **ROS Kinetic** and **Ubuntu 18.04** with **ROS Melodic**. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results.
+A C++11 compiler is needed.
 
-## C++11
-We use all kinds of functionalities from C++11.
-
-## OpenCV
-We use [OpenCV](http://opencv.org) to manipulate images and features. OpenCV should be installed along with ROS.
+## Getting the code
+Clone the repository into your catkin workspace:
+```
+git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git
+```
+## ROS
+This ROS node requires catkin_make_isolated or catkin build to build. This package depends on a number of other ROS packages which ship with the default installation of ROS.
+If they are not installed use [rosdep](http://wiki.ros.org/rosdep) to install them. In your catkin folder run
+```
+sudo rosdep init
+rosdep update
+rosdep install --from-paths src --ignore-src -r -y
+```
+to install all dependencies for all packages. If you already initialized rosdep you get a warning which you can ignore.
 
 ## Eigen3
-Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org.
+Required by g2o. Download and install instructions can be found [here](http://eigen.tuxfamily.org).
 Otherwise Eigen can be installed as a binary with:
 ```
 sudo apt install libeigen3-dev
 ```
-**Required at least 3.1.0**.
+**Required at least Eigen 3.1.0**.
 
 ## Point Cloud Library (PCL)
 We use the [PCL](http://pointclouds.org) to create the dense map. The PCL should come with the full ROS installation. If this is not the case make sure to install it, for example with rosdep.
 
 ## DBoW2 and g2o (Included in Thirdparty folder)
 We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. Both modified libraries (which are BSD) are included in the *Thirdparty* folder.
 
-## ROS / catkin
-This ROS node requires catkin_make_isolated or catkin build to build.
-
-# 3. Building the ORB-SLAM2 ROS node
-## Getting the code
-Clone the repository into your catkin workspace:
-```
-git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git
-```
 ## Building
 To build the node run
 ```
 catkin build
 ```
 in your catkin folder.
 
-# 4. Configuration
-## Config file
-To run the algorithm expects both a vocabulary file (see the paper) and a **config file with the camera- and some hyper parameters**. The vocab file ships with this repository, together with config files for the Intel RealSense r200 camera. If you want to use any other camera you need to adjust the file (you can use one of the provided ones as a template). They are at orb_slam2/config.
+# 3. Configuration
+## Vocab file
+To run the algorithm expects both a vocabulary file (see the paper) which ships with this repository.
 
-## ROS parameters and topics
+# Config
+The config files for camera calibration and tracking hyper paramters from the original implementation are replaced with ros paramters which get set from a launch file.
+
+## ROS parameters, topics and services
 ### Parameters
-In the launch files which can be found at ros/launch there are different parameters which can be adjusted:
+There are three types of parameters right now: static- and dynamic ros parameters and camera settings.
+The static parameters are send to the ROS parameter server at startup and are not supposed to change. They are set in the launch files which are located at ros/launch. The parameters are:
 
+- **load_map**: Bool. If set to true, the node will try to load the map provided with map_file at startup.
+- **map_file**: String. The name of the file the map is loaded from.
+- **voc_file**: String. The location of config vocanulary file mentioned above.
 - **publish_pointcloud**: Bool. If the pointcloud containing all key points (the map) should be published.
-- **localize_only**: Bool. Toggle from/to only localization. The SLAM will then no longer add no new points to the map.
-- **reset_map**: Bool. Set to true to erase the map and start new. After reset the parameter will automatically update back to false.
+- **publish_pose**: Bool. If a PoseStamped message should be published. Even if this is false the tf will still be published.
 - **pointcloud_frame_id**: String. The Frame id of the Pointcloud/map.
 - **camera_frame_id**: String. The Frame id of the camera position.
+- **target_frame_id**: String. Map transform and pose estimate will be provided in this frame id if set.
+- **load_calibration_from_cam**: Bool. If true, camera calibration is read from a `camera_info` topic. Otherwise it is read from launch file params.
+
+Dynamic parameters can be changed at runtime. Either by updating them directly via the command line or by using [rqt_reconfigure](http://wiki.ros.org/rqt_reconfigure) which is the recommended way.
+The parameters are:
+
+- **localize_only**: Bool. Toggle from/to only localization. The SLAM will then no longer add no new points to the map.
+- **reset_map**: Bool. Set to true to erase the map and start new. After reset the parameter will automatically update back to false.
 - **min_num_kf_in_map**: Int. Number of key frames a map has to have to not get reset after tracking is lost.
+- **min_observations_for_ros_map**: Int. Number of minimal observations a key point must have to be published in the point cloud. This doesn't influence the behavior of the SLAM itself at all.
+
+Finally, the intrinsic camera calibration parameters along with some hyperparameters can be found in the specific yaml files in orb_slam2/config.
 
 ### Published topics
 The following topics are being published and subscribed to by the nodes:
 - All nodes publish (given the settings) a **PointCloud2** containing all key points of the map.
+- Also all nodes publish (given the settings) a **PoseStamped** with the current pose of the camera frame, or the target frame if `target_frame_id` is set.
 - Live **image** from the camera containing the currently found key points and a status text.
-- A **tf** from the pointcloud frame id to the camera frame id (the position).
+- A **tf** from the pointcloud frame id to the camera frame id (the position), or the target frame if `target_frame_id` is set.
 
 ### Subscribed topics
-- The mono node subscribes to **/camera/image_raw** for the input image.
+- The mono node subscribes to:
+    - **/camera/image_raw** for the input image
+    - **/camera/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
+
+- The RGBD node subscribes to:
+    - **/camera/rgb/image_raw** for the RGB image
+    - **/camera/depth_registered/image_raw** for the depth information
+    - **/camera/rgb/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
+
+- The stereo node subscribes to:
+    - **image_left/image_color_rect** and
+    - **image_right/image_color_rect** for corresponding images
+    - **image_left/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
 
-- The RGBD node subscribes to **/camera/rgb/image_raw** for the RGB image and
-- **/camera/depth_registered/image_raw** for the depth information.
+# 4. Services
+All nodes offer the possibility to save the map via the service node_type/save_map.
+So the save_map services are:
+- **/orb_slam2_rgbd/save_map**
+- **/orb_slam2_mono/save_map**
+- **/orb_slam2_stereo/save_map**
 
-- The stereo node subscribes to **image_left/image_color_rect** and
-- **image_right/image_color_rect** for corresponding images.
+The save_map service expects the name of the file the map should be saved at as input.
+
+At the moment, while the save to file takes place, the SLAM is inactive.
 
 # 5. Run
 After sourcing your setup bash using
 ```
 source devel/setup.bash
 ```
-you can run the the corresponding nodes with one of the following commands:
+## Suported cameras
+| Camera               | Mono                                                           | Stereo                                                           | RGBD                                                       |
+|----------------------|----------------------------------------------------------------|------------------------------------------------------------------|------------------------------------------------------------|
+| Intel RealSense r200 | ``` roslaunch orb_slam2_ros orb_slam2_r200_mono.launch ```     | ``` roslaunch orb_slam2_ros orb_slam2_r200_stereo.launch ```     | ``` roslaunch orb_slam2_ros orb_slam2_r200_rgbd.launch ``` |
+| Intel RealSense d435 | ``` roslaunch orb_slam2_ros orb_slam2_d435_mono.launch ```     | -                                                                | ``` roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch ``` |
+| Mynteye S            | ```roslaunch orb_slam2_ros orb_slam2_mynteye_s_mono.launch ``` | ```roslaunch orb_slam2_ros orb_slam2_mynteye_s_stereo.launch ``` | -                                                          | 
+| Stereolabs ZED 2     | -                                                              | ```roslaunch orb_slam2_ros orb_slam2_zed2_stereo.launch ```      | -                                                          |                     |                                                            |                                                              |                                                            |
+
+Use the command from the corresponding cell for your camera to launch orb_slam2_ros with the right parameters for your setup.
+
+# 6. Docker
+An easy way is to use orb_slam2_ros with Docker. This repository ships with a Dockerfile based on ROS kinetic.
+The container includes orb_slam2_ros as well as the Intel RealSense package for quick testing and data collection.
+
+# 7. FAQ
+Here are some answers to frequently asked questions.
+### How to save the map
+To save the map with a simple command line command run one the commands (matching to your node running):
+```
+rosservice call /orb_slam2_rgbd/save_map map.bin
+rosservice call /orb_slam2_stereo/save_map map.bin
+rosservice call /orb_slam2_mono/save_map map.bin
+```
+You can replace "map.bin" with any file name you want.
+The file will be saved at ROS_HOME which is by default ~/.ros
+
+**Note** that you need to source your catkin workspace in your terminal in order for the services to become available.
+
+### Using a new / different camera
+You can use this SLAM with almost any mono, stereo or RGBD cam you want.
+In order to use this with a different camera you need to supply a set of paramters to the algorithm. They are loaded from a launch file from the ros/launch folder.
+1) You need the **camera intrinsics and some configurations**. [Here](https://docs.opencv.org/3.1.0/dc/dbb/tutorial_py_calibration.html) you can read about what the camera calibration parameters mean. Use [this](http://wiki.ros.org/camera_calibration) ros node to obtain them for your camera. If you use a stereo or RGBD cam in addition to the calibration and resolution you also need to adjust three other parameters: Camera.bf, ThDepth and DepthMapFactor.
+2) **The ros launch file** which is at ros/launch needs to have the correct topics to subscribe to from the new camera.
+**NOTE** If your camera supports this, orb_slam_2_ros can subscribe to the camera_info topic and read the camera calibration parameters from there.
+
+### Problem running the realsense node
+The node for the RealSense fails to launch when running
+```
+roslaunch realsense2_camera rs_rgbd.launch
+```
+to get the depth stream.
+**Solution:**
+install the rgbd-launch package with the command (make sure to adjust the ROS distro if needed):
 ```
-roslaunch orb_slam2_ros orb_slam2_mono.launch
-roslaunch orb_slam2_ros orb_slam2_stereo.launch
-roslaunch orb_slam2_ros orb_slam2_rgbd.launch
+sudo apt install ros-melodic-rgbd-launch
 ```