diff --git a/.gitignore b/.gitignore index e588a672..f4bc2e88 100755 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ CMakeLists.txt.user +.idea/ +cmake-build-debug KeyFrameTrajectory.txt Thirdparty/DBoW2/build/ Thirdparty/DBoW2/lib/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 96188769..0bd91799 100755 --- a/CMakeLists.txt +++ b/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 ) diff --git a/README.md b/README.md index 27c9885e..e26f2407 100755 --- a/README.md +++ b/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,22 +52,32 @@ 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. @@ -71,15 +85,6 @@ We use the [PCL](http://pointclouds.org) to create the dense map. The PCL should ## 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 ``` @@ -87,44 +92,119 @@ 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 ``` diff --git a/docker/kinetic/Dockerfile b/docker/kinetic/Dockerfile new file mode 100644 index 00000000..da7266fd --- /dev/null +++ b/docker/kinetic/Dockerfile @@ -0,0 +1,42 @@ +FROM ros:kinetic-robot + +# Set working directory +WORKDIR /home + +# Get orb_slam_2_ros and the realsense package from git +RUN mkdir src +RUN cd src \ +&& git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git \ +&& git clone https://github.com/IntelRealSense/realsense-ros.git + + +# Set up Kinetic keys +RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +# Set up realsense keys +RUN apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE +RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + +# Update +RUN apt update +RUN apt-get install software-properties-common apt-utils -y + +#Add realsense repo +RUN add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u + +# Install required realsense and ROS packages +RUN apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev \ + librealsense2-dbg ros-kinetic-rgbd-launch ros-kinetic-tf2-geometry-msgs python-catkin-tools -y + +# Install ROS dependencies +RUN rosdep update \ +&& rosdep install --from-paths src --ignore-src -r -y --skip-keys=librealsense2 + +# build ros package source +RUN catkin config \ + --extend /opt/ros/$ROS_DISTRO && \ + catkin build + +RUN echo "source /home/devel/setup.bash" >> /$HOME/.bashrc + +CMD "bash" \ No newline at end of file diff --git a/docker/melodic/Dockerfile b/docker/melodic/Dockerfile new file mode 100644 index 00000000..8fc94b8a --- /dev/null +++ b/docker/melodic/Dockerfile @@ -0,0 +1,41 @@ +FROM ros:melodic-robot + +# Update +RUN apt update +RUN apt-get install software-properties-common apt-utils -y + +# Set working directory +WORKDIR /home/ros/src + +# Get the realsense package from git +RUN git clone https://github.com/IntelRealSense/realsense-ros.git +RUN git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git + +# Set up Melodic keys +RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + +# Set up realsense keys +RUN apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE +RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + +#Add realsense repo +RUN add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u + +# Install required realsense and ROS packages +RUN apt-get update && \ + apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg ros-melodic-tf2-geometry-msgs python-catkin-tools -y + +WORKDIR /home/ros + +# Install ROS dependencies +RUN rosdep update \ +&& rosdep install --from-paths src --ignore-src -r -y --skip-keys=librealsense2 + +# build ros package source +RUN catkin config \ + --extend /opt/ros/$ROS_DISTRO && \ + catkin build + +RUN echo "source /home/ros/devel/setup.bash" >> /$HOME/.bashrc + +CMD "bash" \ No newline at end of file diff --git a/orb_slam2/Thirdparty/mt_task_queue/include/Task.h b/orb_slam2/Thirdparty/mt_task_queue/include/Task.h index e5043ee1..42451a99 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/Task.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/Task.h @@ -9,21 +9,44 @@ For more information see the README. #include +// required for explicit template instantiation in Task.cpp +#include +#include +#include +#include + namespace TaskQueue { template class Task { public: - Task (unsigned int task_id, unsigned int priority, std::function task_function); + /* + * Constructor will require task_id, priority, the task function to run and of course the arguments to pass to that function. + */ + Task (unsigned int task_id, unsigned int priority, std::function task_function, Args... function_args); - friend bool operator < (const Task& left, const Task& right) { - return left.GetPriority() < right.GetPriority(); + /* + * This operator cannot be "friend bool operator < ..." because then we can't define it as a const function. + * And we need this operator to be const to tell the compiler that it will not change the class members- this + * is required when Task objects are copied or moved (e.g. pushed or popped from lists or passed by reference I think). + */ + bool operator < (const Task& other) const { + return GetPriority() < other.GetPriority(); } - friend bool operator > (const Task& left, const Task& right) { - return left.GetPriority() > right.GetPriority(); + + /* + * This operator cannot be "friend bool operator > ..." because then we can't define it as a const function. + * And we need this operator to be const to tell the compiler that it will not change the class members- this + * is required when Task objects are copied or moved (e.g. pushed or popped from lists or passed by reference I think). + */ + bool operator > (const Task& other) const { + return GetPriority() > other.GetPriority(); } - unsigned int GetPriority () {return priority_;} + /* + * This function has to be const to allow the > and < operators to be const. + */ + unsigned int GetPriority () const {return priority_;} unsigned int GetId () {return task_id_;} bool HasReturnValue () {return has_return_value_;} ReturnType GetResult () {return result_;} @@ -33,6 +56,10 @@ class Task { unsigned int task_id_; unsigned int priority_; std::function task_function_; + /* + * Alongside the task function and its returned result, we also need its parameters. + */ + std::tuple function_args_; ReturnType result_; bool has_return_value_; }; diff --git a/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h b/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h index 309c4ade..49ace5bb 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h @@ -19,6 +19,12 @@ For more information see the README. #include "Task.h" #include "Worker.h" +// required for explicit template instantiation in TaskQueue.cpp +#include +#include +#include +#include + namespace TaskQueue { template @@ -27,17 +33,34 @@ class TaskQueue { TaskQueue (unsigned int num_worker_threads); ~TaskQueue (); - void AddTask (std::function task_function) {AddTask (1, task_function);} - void AddTask (unsigned int priority, std::function task_function) {AddTask (0, 1, task_function);} - void AddTask (unsigned int task_id, unsigned int priority, std::function task_function); - Task GetTask (unsigned int task_id); + /* + * AddTask function as a bare minimum requires the task function alongside its arguments. Priority and task_id need to be given + * or can be defaulted. + */ + void AddTask (unsigned int task_id, unsigned int priority, std::function task_function, Args... function_args); + void AddTask (unsigned int priority, std::function task_function, Args... function_args) { + AddTask(0, priority, task_function, function_args...); + } + void AddTask (std::function task_function, Args... function_args) { + AddTask(1, 0, task_function, function_args...); + } + + /* + * The task that we return has to be a pointer to the Task instance because otherwise a copy construction of Task is required + * when we call this function and copy construction of task can be problematic due to atomic functions and thread locks inside it, + * which can't be easily copied. + */ + Task* GetTask (unsigned int task_id); bool ResultAvailable (unsigned int task_id); unsigned int NumJobsCurrentlyRunning (); private: std::priority_queue , std::vector>, std::greater>> task_queue_; - std::vector> workers_; - std::map> results_; + std::vector*> workers_; + /* + * The results_ buffer has to contain pointers to Task objects because our GetTask function now returns a pointer rather than a Task object. + */ + std::map*> results_; std::mutex queue_mutex_; std::mutex map_mutex_; std::condition_variable condition_var_; diff --git a/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h b/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h index 88c4bac5..99253892 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h @@ -23,18 +23,36 @@ namespace TaskQueue { template class Worker { public: + /* + * In the constructor to Worker we must pass pointers to map_mutex, queue_mutex and the results buffer must contain pointers + * to Task objects because otherwise these mutexes and Tasks would have to be copy-constructed, which is problematic. + */ Worker (std::priority_queue, std::vector>, std::greater>> &task_queue, - std::map> &results, std::mutex &queue_mutex, std::mutex &map_mutex, std::condition_variable &condition_var); + std::map*> &results, std::mutex* queue_mutex, std::mutex* map_mutex, std::condition_variable &condition_var); + + /* + * We need a move constructor, because somewhere in our code we are moving Worker objects (returning them from some function probably). + */ + Worker(Worker&&); void EndWorker() {end_operator_flag_ = true;} bool IsIdeling () {return idle_;} + ~Worker(); + + private: std::priority_queue , std::vector>, std::greater>> &task_queue_; - std::map> &results_; - std::mutex &queue_mutex_; - std::mutex &map_mutex_; + /* + * The results_ buffer must contain pointers to Task objects. + */ + std::map*> &results_; + /* + * Mutex objects need to be pointed at instead of referred to because otherwise we need to copy-construct them in this class constructor. + */ + std::mutex* queue_mutex_; + std::mutex* map_mutex_; std::mutex thread_mutex_; std::condition_variable &condition_var_; std::atomic end_operator_flag_; diff --git a/orb_slam2/Thirdparty/mt_task_queue/src/Task.cpp b/orb_slam2/Thirdparty/mt_task_queue/src/Task.cpp index ad96f5f8..39d9d372 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/Task.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/Task.cpp @@ -3,8 +3,11 @@ namespace TaskQueue { template -Task::Task (unsigned int task_id, unsigned int priority, std::function task_function) -: task_id_(task_id), priority_(priority), task_function_(task_function) { +/* + * Task constructor must receive not only the task function, but also its arguments. + */ +Task::Task (unsigned int task_id, unsigned int priority, std::function task_function, Args... function_args) +: task_id_(task_id), priority_(priority), task_function_(task_function), function_args_(function_args...) { if (task_function_.target_type() != typeid(void(*)(Args...))) { //right? TODO, WARNING, fuck, i've no idea what im doing has_return_value_ = true; } else { @@ -12,15 +15,38 @@ Task::Task (unsigned int task_id, unsigned int priority, st } } +/* + * The function required to unpack passed function arguments and call a passed function with those arguments. + * This is essentially doing the work of std::apply, which we could use in C++17, but we have C++14, so need + * this. + */ +template +decltype(auto) CallFunctionWithTuple(Func&& func, Tuple&& args, std::index_sequence) { + return func(std::get(std::forward(args))...); +} template -void Task::Task::RunTask () { +void Task::RunTask () { if (has_return_value_) { - result_ = task_function_ (); + /* + * Unpack and call the stored function. And because the function parameters are in a tuple, we need + * to unpack them with this CallFunctionWithTuple function. If this was C++17 or higher, then we could + * use std::apply, but this is C++14. + */ + result_ = CallFunctionWithTuple(task_function_, function_args_, std::index_sequence_for()); } else { - task_function_ (); + /* + * Unpack and call the stored function. And because the function parameters are in a tuple, we need + * to unpack them with this CallFunctionWithTuple function. If this was C++17 or higher, then we could + * use std::apply, but this is C++14. + */ + CallFunctionWithTuple(task_function_, function_args_, std::index_sequence_for()); } } +// Explicit template instantiation for the supported types. This is needed so that the rest of the project can link to this module. If this is not here, then support for the +// required types will not be generated by the compiler. +template class Task>, boost::shared_ptr>, boost::shared_ptr>, cv::Mat>; + } // namespace TaskQueue diff --git a/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp b/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp index 456ef589..ee06b67f 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp @@ -5,43 +5,57 @@ namespace TaskQueue { template TaskQueue::TaskQueue (unsigned int num_worker_threads) { if (num_worker_threads == 0) { - throw std::invalid_argument ("Cannot launch threat queue with 0 workers"); + throw std::invalid_argument ("Cannot launch thread queue with 0 workers"); } - for (int i : num_worker_threads) { - workers_.push_back (Worker (task_queue_, results_, queue_mutex_, map_mutex_, condition_var_)); + for (unsigned int i = 0; i < num_worker_threads; i++) { + workers_.push_back (new Worker (task_queue_, results_, &queue_mutex_, &map_mutex_, condition_var_)); } } + template TaskQueue::~TaskQueue () { - for (auto worker : workers_) { - worker.EndWorker(); + /* + * We need to iterate through the workers_ collection in this way- to avoid taking the Worker objects + * out of the queue, which would cause copy-construction. + */ + for (unsigned int i = 0; i < workers_.size(); i++) { + //std::cout << "AE: TQ DESTR" << std::endl; + workers_[i]->EndWorker(); } } template -void TaskQueue::AddTask (unsigned int task_id, unsigned int priority, std::function task_function) { +/* + * AddTask needs to receive the task function alongside its parameters as a minimum. + */ +void TaskQueue::AddTask (unsigned int task_id, unsigned int priority, std::function task_function, Args... function_args) { std::unique_lock lock(queue_mutex_); - task_queue_.push(Task (task_id, priority, task_function)); + task_queue_.push(Task (task_id, priority, task_function, function_args...)); condition_var_.notify_one(); } template -Task TaskQueue::GetTask (unsigned int task_id) { +/* + * GetTask must return a pointer to the Task object. + */ +Task* TaskQueue::GetTask (unsigned int task_id) { std::unique_lock lock(map_mutex_); auto it = results_.find (task_id); if (it != results_.end()) { - return results_[task_id]; + /* + * We want to return the pointer to the Task object without getting it out of the resuts_ map, which would cause copy-constructing. + */ + return it->second; // Return the existing Task object from the map } else { - throw std::bad_exception ("Result not (yet?) available"); + throw std::runtime_error("Result not (yet?) available"); } } - template bool TaskQueue::ResultAvailable (unsigned int task_id) { std::unique_lock lock(map_mutex_); @@ -64,12 +78,20 @@ bool TaskQueue::QueueIsEmpty () { template unsigned int TaskQueue::NumJobsCurrentlyRunning () { unsigned int workers_working = 0; - for (auto worker : workers_) { - if (worker.IsIdeling()) { + /* + * We need to iterate through the workers_ collection in this way- to avoid taking the Worker objects + * out of the queue, which would cause copy-construction. + */ + for (unsigned int i = 0; i < workers_.size(); i++) { + if (!workers_[i]->IsIdeling()) { //# if (workers_[i].IsIdeling()) { //# surely we want to count workers that are NOT idle though??? workers_working ++; } } return workers_working; } +// Explicit template instantiation for the supported types. This is needed so that the rest of the project can link to this module. If this is not here, then support for the +// required types will not be generated by the compiler. This is what template does for the TaskQueue object. +template class TaskQueue >, boost::shared_ptr >, boost::shared_ptr >, cv::Mat>; + } // namespace TaskQueue diff --git a/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp b/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp index ee1acb99..47c82b2e 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp @@ -3,17 +3,57 @@ namespace TaskQueue { template +/* + * Arguments to constructor now contain mutex pointers because mutexes are problematic to copy. results map also + * now contains pointers to Task objects rather than objects themselves- to avoid unnecessary copy-construction. + */ Worker::Worker (std::priority_queue, std::vector>, std::greater>> &task_queue, - std::map> &results, std::mutex &queue_mutex, std::mutex &map_mutex, + std::map*> &results, std::mutex* queue_mutex, std::mutex* map_mutex, std::condition_variable &condition_var) - : task_queue_(task_queue), results_(results), queue_mutex_(queue_mutex), map_mutex_(map_mutex), condition_var_(condition_var) { + : task_queue_(task_queue), results_(results), condition_var_(condition_var) { end_operator_flag_ = false; idle_ = true; - Operator (); + queue_mutex_ = queue_mutex; + map_mutex_ = map_mutex; + + /* + * Each worker has to run its own separate thread because otherwise we will never get out of constructor here since the + * Operator function blocks for most of the time and certainly at the beginning. + */ + std::thread tWorker(&Worker::Operator, this); + //Operator (); + tWorker.detach(); } +template +Worker::~Worker () { + std::cout << "AE: WORKER DESTR" << std::endl; +} + +/** + * Move constructor to deal with the mutexes that can't be copied. + * + * Really we should not execute this until we're sure that Operator () has completed execution. So really either + * throw an exception or block if other.idle_ == false or if the mutexes are locked. But again, since we are only + * creating 1 Worker for now, I'll let this slide. + */ +template +Worker::Worker(Worker&& other) : task_queue_(other.task_queue_), results_(other.results_), condition_var_(other.condition_var_) { + // mutexes can be just assigned as they are only pointers + queue_mutex_ = other.queue_mutex_; + map_mutex_ = other.map_mutex_; + // thread_mutex_ will be created brand new just as in the normal constructor. That one is only used to acquire unique_lock in the Operator () + + /* + * atomic fields cannot be assigned from existing atomic field because that would cause copy-construction and atomic types do not like + * to be copy-constructed. Therefore we use the load() function to get their bool value. + */ + end_operator_flag_ = other.end_operator_flag_.load(); + idle_ = other.idle_.load(); + // worker_thread_ will also be created brand new- not sure what it is used for, I think it is not used, but we'll deal with that later. That happens automatically. +} template void Worker::Operator () { @@ -21,27 +61,46 @@ void Worker::Operator () { while (!end_operator_flag_) { idle_ = true; condition_var_.wait(lock); - - queue_mutex_.lock(); + std::cout << "AE2.1.2.2.4" << std::endl; + /* + * Since mutexes are now passed as pointers, we need pointers' access operator -> . + */ + queue_mutex_->lock(); + std::cout << "AE2.1.2.2.5" << std::endl; if (task_queue_.empty()) { - queue_mutex_.unlock(); + queue_mutex_->unlock(); continue; } idle_ = false; - + std::cout << "AE2.1.2.2.6" << std::endl; Task top_task = task_queue_.top(); + std::cout << "AE2.1.2.2.7" << std::endl; task_queue_.pop(); - queue_mutex_.unlock(); + queue_mutex_->unlock(); + std::cout << "AE2.1.2.2.8" << std::endl; top_task.RunTask (); + std::cout << "AE2.1.2.2.9" << std::endl; if (top_task.HasReturnValue()) { - map_mutex_.lock(); - results_[top_task.GetId()] = top_task; - map_mutex_.unlock(); + /* + * Since mutexes are now passed as pointers, we need pointers' access operator -> . + */ + map_mutex_->lock(); + results_[top_task.GetId()] = &top_task; + map_mutex_->unlock(); } + std::cout << "AE2.1.2.2.10" << std::endl; } + /* + * remove itself when all mutexes and conditional vars are no longer in use. + */ + delete this; } +// Explicit template instantiation for the supported types. This is needed so that the rest of the project can link to this module. If this is not here, then support for the +// required types will not be generated by the compiler. This is what template does for the Worker object. +template class Worker>, boost::shared_ptr>, boost::shared_ptr>, cv::Mat>; + } // namespace TaskQueue diff --git a/orb_slam2/config/RealSenseMono.yaml b/orb_slam2/config/RealSenseMono.yaml deleted file mode 100644 index 36f91d1c..00000000 --- a/orb_slam2/config/RealSenseMono.yaml +++ /dev/null @@ -1,43 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) - -Camera.fx: 632.7927856445312 -Camera.fy: 626.8605346679688 -Camera.cx: 311.43603515625 -Camera.cy: 248.0950164794922 - -Camera.k1: -0.09097914397716522 -Camera.k2: 0.06503549218177795 -Camera.p1: 0.000849052332341671 -Camera.p2: 0.001785792293958366 -Camera.k3: 0.0 - -Camera.width: 640 -Camera.height: 480 - -# Camera frames per second -Camera.fps: 60.0 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 1 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 2000 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 diff --git a/orb_slam2/config/RealSenseRGBD.yaml b/orb_slam2/config/RealSenseRGBD.yaml deleted file mode 100644 index aa1d0e30..00000000 --- a/orb_slam2/config/RealSenseRGBD.yaml +++ /dev/null @@ -1,52 +0,0 @@ -%YAML:1.0 - -# Camera calibration parameters (OpenCV) -Camera.fx: 632.7927 -Camera.fy: 626.8605 -Camera.cx: 320.0 -Camera.cy: 240.0 - -# Camera distortion paremeters (OpenCV) -- -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 - -Camera.width: 640 -Camera.height: 480 - -# IR projector baseline times fx (aprox.) -Camera.bf: 37.2925 - -# Camera frames per second -Camera.fps: 30.0 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 1 - -# Close/Far threshold. Baseline times. -ThDepth: 10.0 - -# Deptmap values factor -DepthMapFactor: 1.0 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1000 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 diff --git a/orb_slam2/config/RealSenseStereo.yaml b/orb_slam2/config/RealSenseStereo.yaml deleted file mode 100644 index 06d37a86..00000000 --- a/orb_slam2/config/RealSenseStereo.yaml +++ /dev/null @@ -1,102 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) - -Camera.fx: 454.2463073730469 -Camera.fy: 454.2463073730469 -Camera.cx: 231.73892211914062 -Camera.cy: 179.4013214111328 - -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 -Camera.k3: 0.0 - -Camera.width: 480 -Camera.height: 360 - -# Camera frames per second -Camera.fps: 30.0 - -# stereo baseline (distance between cameras in m?) times fx -Camera.bf: 30.464328718 # 40 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 1 - -# Close/Far threshold. Baseline times. -ThDepth: 30 # what is this? - -# seen elsewhere... what is this? -#DepthMapFactor: 1000.0 - -#-------------------------------------------------------------------------------------------- -# Stereo Rectification. Only if you need to pre-rectify the images. -# Camera.fx, .fy, etc must be the same as in LEFT.P -#-------------------------------------------------------------------------------------------- -LEFT.height: 360 -LEFT.width: 480 -LEFT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [0.0, 0.0, 0.0, 0.0, 0.0] -LEFT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [454.2463073730469, 0.0, 231.73892211914062, 0.0, 454.2463073730469, 179.4013214111328, 0.0, 0.0, 1.0] -LEFT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] -LEFT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [454.2463073730469, 0.0, 231.73892211914062, 0.0, 0.0, 454.2463073730469, 179.4013214111328, 0.0, 0.0, 0.0, 1.0, 0.0] - -RIGHT.height: 360 -RIGHT.width: 480 -RIGHT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [0.0, 0.0, 0.0, 0.0, 0.0] -RIGHT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [454.2463073730469, 0.0, 231.73892211914062, 0.0, 454.2463073730469, 179.4013214111328, 0.0, 0.0, 1.0] -RIGHT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] -RIGHT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [454.2463073730469, 0.0, 231.73892211914062, 0.0, 0.0, 454.2463073730469, 179.4013214111328, 0.0, 0.0, 0.0, 1.0, 0.0] - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1200 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 diff --git a/orb_slam2/config/TUM1.yaml b/orb_slam2/config/TUM1.yaml deleted file mode 100644 index 8fb904a5..00000000 --- a/orb_slam2/config/TUM1.yaml +++ /dev/null @@ -1,51 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 517.306408 -Camera.fy: 516.469215 -Camera.cx: 318.643040 -Camera.cy: 255.313989 - -Camera.k1: 0.262383 -Camera.k2: -0.953104 -Camera.p1: -0.005358 -Camera.p2: 0.002628 -Camera.k3: 1.163314 - -Camera.width: 640 -Camera.height: 480 - -# Camera frames per second -Camera.fps: 30.0 - -# IR projector baseline times fx (aprox.) -Camera.bf: 40.0 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 1 - -# Close/Far threshold. Baseline times. -ThDepth: 40.0 - -# Deptmap values factor -DepthMapFactor: 5000.0 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1000 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 20 -ORBextractor.minThFAST: 7 diff --git a/orb_slam2/config/TUM2.yaml b/orb_slam2/config/TUM2.yaml new file mode 100644 index 00000000..4f471f48 --- /dev/null +++ b/orb_slam2/config/TUM2.yaml @@ -0,0 +1,68 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 535.4 +Camera.fy: 539.2 +Camera.cx: 320.1 +Camera.cy: 247.6 + +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.width: 640 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30.0 + +# IR projector baseline times fx (aprox.) +Camera.bf: 40.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 0 + +# Close/Far threshold. Baseline times. +ThDepth: 40.0 + +# Deptmap values factor +DepthMapFactor: 1 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 diff --git a/orb_slam2/config/vimba_stereo.yaml b/orb_slam2/config/vimba_stereo.yaml deleted file mode 100644 index 1082c5b3..00000000 --- a/orb_slam2/config/vimba_stereo.yaml +++ /dev/null @@ -1,48 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 1208.226731 -Camera.fy: 1205.548227 -Camera.cx: 305.863428 -Camera.cy: 283.394157 - -Camera.k1: -0.087742 -Camera.k2: 0.374604 -Camera.p1: 0.000112 -Camera.p2: 0.000612 -Camera.k3: 0.000000 - -Camera.width: 544 -Camera.height: 544 - -# Camera frames per second -Camera.fps: 30.0 - -# stereo baseline times fx -Camera.bf: 86.14365333630209 - -# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -Camera.RGB: 0 - -# Close/Far threshold. Baseline times. -ThDepth: 25 - -#-------------------------------------------------------------------------------------------- -# ORB Parameters -#-------------------------------------------------------------------------------------------- - -# ORB Extractor: Number of features per image -ORBextractor.nFeatures: 1200 - -# ORB Extractor: Scale factor between levels in the scale pyramid -ORBextractor.scaleFactor: 1.2 - -# ORB Extractor: Number of levels in the scale pyramid -ORBextractor.nLevels: 8 - -# ORB Extractor: Fast threshold -# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. -# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST -# You can lower these values if your images have low contrast -ORBextractor.iniThFAST: 10 -ORBextractor.minThFAST: 4 diff --git a/orb_slam2/include/BoostArchiver.h b/orb_slam2/include/BoostArchiver.h new file mode 100644 index 00000000..3e102a24 --- /dev/null +++ b/orb_slam2/include/BoostArchiver.h @@ -0,0 +1,95 @@ +/* + * map save/load extension for ORB_SLAM2 + * This header contains boost headers needed by serialization + * + * object to save: + * - KeyFrame + * - KeyFrameDatabase + * - Map + * - MapPoint + */ +#ifndef BOOST_ARCHIVER_H +#define BOOST_ARCHIVER_H +#include +#include +#include +// set serialization needed by KeyFrame::mspChildrens ... +#include +// map serialization needed by KeyFrame::mConnectedKeyFrameWeights ... +#include +#include +#include +#include +// base object needed by DBoW2::BowVector and DBoW2::FeatureVector +#include + +#include "Thirdparty/DBoW2/DBoW2/BowVector.h" +#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" + +BOOST_SERIALIZATION_SPLIT_FREE(::cv::Mat) +namespace boost{ + namespace serialization { + + /* serialization for DBoW2 BowVector */ + template + void serialize(Archive &ar, DBoW2::BowVector &BowVec, const unsigned int file_version) + { + ar & boost::serialization::base_object< std::map >(BowVec); + } + /* serialization for DBoW2 FeatureVector */ + template + void serialize(Archive &ar, DBoW2::FeatureVector &FeatVec, const unsigned int file_version) + { + ar & boost::serialization::base_object > >(FeatVec); + } + + /* serialization for CV KeyPoint */ + template + void serialize(Archive &ar, ::cv::KeyPoint &kf, const unsigned int file_version) + { + ar & kf.angle; + ar & kf.class_id; + ar & kf.octave; + ar & kf.response; + ar & kf.response; + ar & kf.pt.x; + ar & kf.pt.y; + } + /* serialization for CV Mat */ + template + void save(Archive &ar, const ::cv::Mat &m, const unsigned int file_version) + { + cv::Mat m_ = m; + if (!m.isContinuous()) + m_ = m.clone(); + size_t elem_size = m_.elemSize(); + size_t elem_type = m_.type(); + ar & m_.cols; + ar & m_.rows; + ar & elem_size; + ar & elem_type; + + const size_t data_size = m_.cols * m_.rows * elem_size; + + ar & boost::serialization::make_array(m_.ptr(), data_size); + } + template + void load(Archive & ar, ::cv::Mat& m, const unsigned int version) + { + int cols, rows; + size_t elem_size, elem_type; + + ar & cols; + ar & rows; + ar & elem_size; + ar & elem_type; + + m.create(rows, cols, elem_type); + size_t data_size = m.cols * m.rows * elem_size; + + ar & boost::serialization::make_array(m.ptr(), data_size); + } + } +} +// TODO: boost::iostream zlib compressed binary format +#endif // BOOST_ARCHIVER_H diff --git a/orb_slam2/include/KeyFrame.h b/orb_slam2/include/KeyFrame.h index 67f43482..8797515e 100755 --- a/orb_slam2/include/KeyFrame.h +++ b/orb_slam2/include/KeyFrame.h @@ -28,6 +28,7 @@ #include "ORBextractor.h" #include "Frame.h" #include "KeyFrameDatabase.h" +#include "BoostArchiver.h" #include @@ -231,6 +232,17 @@ class KeyFrame std::mutex mMutexPose; std::mutex mMutexConnections; std::mutex mMutexFeatures; + +// map serialization addition +public: + KeyFrame(); // Default constructor for serialization, need to deal with const member + void SetORBvocabulary(ORBVocabulary *porbv) {mpORBvocabulary=porbv;} +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + }; } //namespace ORB_SLAM diff --git a/orb_slam2/include/KeyFrameDatabase.h b/orb_slam2/include/KeyFrameDatabase.h index fa373576..35a3c1eb 100755 --- a/orb_slam2/include/KeyFrameDatabase.h +++ b/orb_slam2/include/KeyFrameDatabase.h @@ -28,6 +28,7 @@ #include "KeyFrame.h" #include "Frame.h" #include "ORBVocabulary.h" +#include "BoostArchiver.h" #include @@ -67,6 +68,18 @@ class KeyFrameDatabase // Mutex std::mutex mMutex; + +// map serialization addition +public: + // for serialization + KeyFrameDatabase() {} + void SetORBvocabulary(ORBVocabulary *porbv) {mpVoc=porbv;} +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); + }; } //namespace ORB_SLAM diff --git a/orb_slam2/include/LoopClosing.h b/orb_slam2/include/LoopClosing.h index 7eb0416b..c3adf13a 100755 --- a/orb_slam2/include/LoopClosing.h +++ b/orb_slam2/include/LoopClosing.h @@ -47,7 +47,7 @@ class LoopClosing typedef pair,int> ConsistentGroup; typedef map, - Eigen::aligned_allocator > > KeyFrameAndPose; + Eigen::aligned_allocator > > KeyFrameAndPose; public: diff --git a/orb_slam2/include/Map.h b/orb_slam2/include/Map.h index a75339fe..507f43c1 100755 --- a/orb_slam2/include/Map.h +++ b/orb_slam2/include/Map.h @@ -23,8 +23,9 @@ #include "MapPoint.h" #include "KeyFrame.h" -#include +#include "BoostArchiver.h" +#include #include @@ -78,6 +79,13 @@ class Map int mnBigChangeIdx; std::mutex mMutexMap; + +// map serialization addition +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); }; } //namespace ORB_SLAM diff --git a/orb_slam2/include/MapPoint.h b/orb_slam2/include/MapPoint.h index f26893d8..aca0ccb7 100755 --- a/orb_slam2/include/MapPoint.h +++ b/orb_slam2/include/MapPoint.h @@ -24,6 +24,7 @@ #include"KeyFrame.h" #include"Frame.h" #include"Map.h" +#include "BoostArchiver.h" #include #include @@ -145,6 +146,16 @@ class MapPoint std::mutex mMutexPos; std::mutex mMutexFeatures; + +// map serialization addition +public: + // for serialization + MapPoint(); +private: + // serialize is recommended to be private + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version); }; } //namespace ORB_SLAM diff --git a/orb_slam2/include/ORBextractor.h b/orb_slam2/include/ORBextractor.h index 66e8e7a5..3acc0e42 100755 --- a/orb_slam2/include/ORBextractor.h +++ b/orb_slam2/include/ORBextractor.h @@ -23,8 +23,8 @@ #include #include -#include +#include namespace ORB_SLAM2 { diff --git a/orb_slam2/include/PnPsolver.h b/orb_slam2/include/PnPsolver.h index f92544fc..b2aa6eac 100755 --- a/orb_slam2/include/PnPsolver.h +++ b/orb_slam2/include/PnPsolver.h @@ -52,9 +52,14 @@ #define PNPSOLVER_H #include +#include #include "MapPoint.h" #include "Frame.h" +#include +#include +using namespace cv; + namespace ORB_SLAM2 { diff --git a/orb_slam2/include/Sim3Solver.h b/orb_slam2/include/Sim3Solver.h index 9af66cb2..81f16db7 100755 --- a/orb_slam2/include/Sim3Solver.h +++ b/orb_slam2/include/Sim3Solver.h @@ -23,11 +23,14 @@ #define SIM3SOLVER_H #include +#include #include #include "KeyFrame.h" - +#include +#include +using namespace cv; namespace ORB_SLAM2 { diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index bc06a1eb..e7f1f030 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -22,10 +22,12 @@ #ifndef SYSTEM_H #define SYSTEM_H -#include -#include +#include +#include #include -#include +#include +#include + #include "Tracking.h" #include "FrameDrawer.h" @@ -36,6 +38,10 @@ #include "ORBVocabulary.h" #include "DenseMap.h" +#include +#include +//using namespace cv; + namespace ORB_SLAM2 { class FrameDrawer; @@ -44,6 +50,8 @@ class Tracking; class LocalMapping; class LoopClosing; +struct ORBParameters; + class System { public: @@ -57,9 +65,10 @@ class System public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. - System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor); + System(const string strVocFile, const eSensor sensor, ORBParameters& parameters, + const std::string & map_file = "", bool load_map = false); // map serialization addition - // Proccess the given stereo frame. Images must be synchronized and rectified. + // Process the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). void TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp); @@ -70,7 +79,7 @@ class System // Returns the camera pose (empty if tracking fails). void TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp); - // Proccess the given monocular frame + // Process the given monocular frame // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). void TrackMonocular(const cv::Mat &im, const double ×tamp); @@ -79,6 +88,9 @@ class System // since last call to this function bool MapChanged(); + // Returns true if Global Bundle Adjustment is running + bool isRunningGBA(); + // Reset the system (clear map) void Reset(); @@ -114,6 +126,8 @@ class System void SetMinimumKeyFrames (int min_num_kf); + bool SaveMap(const string &filename); + cv::Mat GetCurrentPosition (); // Information from most recent processed frame @@ -127,14 +141,24 @@ class System std::vector GetAllMapPoints(); private: + bool SetCallStackSize (const rlim_t kNewStackSize); + + rlim_t GetCurrentCallStackSize (); + // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); // This resumes local mapping thread and performs SLAM again. void DeactivateLocalizationMode(); + bool LoadMap(const string &filename); + bool currently_localizing_only_; + bool load_map; + + std::string map_file; + // Input sensor eSensor mSensor; @@ -186,6 +210,7 @@ class System // Current position cv::Mat current_position_; + }; }// namespace ORB_SLAM diff --git a/orb_slam2/include/Tracking.h b/orb_slam2/include/Tracking.h index 730e2264..acbcef8e 100644 --- a/orb_slam2/include/Tracking.h +++ b/orb_slam2/include/Tracking.h @@ -24,6 +24,11 @@ #include #include +#include + +#include +#include + #include"FrameDrawer.h" #include"Map.h" @@ -49,12 +54,22 @@ class LocalMapping; class LoopClosing; class System; +struct ORBParameters{ + // general parameters for the ORB detector + int maxFrames, nFeatures, nLevels, iniThFAST, minThFAST; + bool RGB; + float scaleFactor, depthMapFactor, thDepth; + // camera parameters + float fx, fy, cx, cy, baseline; + float k1, k2, p1, p2, k3; +}; + class Tracking { public: Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, Map* pMap, - DenseMap* dense_map, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); + DenseMap* dense_map, KeyFrameDatabase* pKFDB, const int sensor, ORBParameters& parameters); // Preprocess the input and call Track(). Extract features and performs stereo matching. cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); @@ -219,6 +234,14 @@ class Tracking bool mbRGB; list mlpTemporalPoints; + + + // These parameters are for the ORB features extractor + int nFeatures; + float fScaleFactor; + int nLevels; + int fIniThFAST; + int fMinThFAST; }; } //namespace ORB_SLAM diff --git a/orb_slam2/src/DenseMap.cc b/orb_slam2/src/DenseMap.cc index 28be5f05..d121b8b5 100644 --- a/orb_slam2/src/DenseMap.cc +++ b/orb_slam2/src/DenseMap.cc @@ -7,6 +7,9 @@ DenseMap::DenseMap () { is_new_ = false; create_dense_map_ = true; current_task_id_ = 0; + /* + * Constructing a TaskQueue object. Not sure yet how and where we use it. + */ task_queue_ = new TaskQueue::TaskQueue (1); } @@ -50,17 +53,17 @@ void DenseMap::AddFrameToMap (unsigned long int frame_id) { MatsToPclRGBDCloud (depth_map_, rgb_map_, map); DenseMap::PointCloudRGBD::Ptr frame (new DenseMap::PointCloudRGBD); MatsToPclRGBDCloud (raw_map_data_[frame_id].depth_img, raw_map_data_[frame_id].rgb_img, frame); - + std::cout << "AE: AddFrameToMap 1 " << std::endl; while (task_queue_->NumJobsCurrentlyRunning () > 0) { } //Wait until current job has finished - + std::cout << "AE: AddFrameToMap 2 " << std::endl; //std::function //auto test = std::bind(&DenseMap::FitFrame, this, map, frame, raw_map_data_[frame_id].position); //DenseMap::FitFrame (map, frame, raw_map_data_[frame_id].position); //&DenseMap::FitFrame; //task_queue_->AddTask (current_task_id_, 1, DenseMap::FitFrame); //task_queue_->AddTask (current_task_id_, 1, test); PclRGBDCloudToMats (map, depth_map_, rgb_map_); - + std::cout << "AE: AddFrameToMap 3 " << std::endl; is_new_ = true; } diff --git a/orb_slam2/src/KeyFrame.cc b/orb_slam2/src/KeyFrame.cc index 4ef1e78e..9791dc3a 100755 --- a/orb_slam2/src/KeyFrame.cc +++ b/orb_slam2/src/KeyFrame.cc @@ -662,4 +662,89 @@ float KeyFrame::ComputeSceneMedianDepth(const int q) return vDepths[(vDepths.size()-1)/q]; } +// map serialization addition +// Default serializing Constructor +KeyFrame::KeyFrame(): + mnFrameId(0), mTimeStamp(0.0), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), + mfGridElementWidthInv(0.0), mfGridElementHeightInv(0.0), + mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), + mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), + fx(0.0), fy(0.0), cx(0.0), cy(0.0), invfx(0.0), invfy(0.0), + mbf(0.0), mb(0.0), mThDepth(0.0), N(0), mnScaleLevels(0), mfScaleFactor(0), + mfLogScaleFactor(0.0), + mnMinX(0), mnMinY(0), mnMaxX(0), + mnMaxY(0) +{} +template +void KeyFrame::serialize(Archive &ar, const unsigned int version) +{ + // no mutex needed vars + ar & nNextId; + ar & mnId; + ar & const_cast(mnFrameId); + ar & const_cast(mTimeStamp); + // Grid related vars + ar & const_cast(mnGridCols); + ar & const_cast(mnGridRows); + ar & const_cast(mfGridElementWidthInv); + ar & const_cast(mfGridElementHeightInv); + // Tracking related vars + ar & mnTrackReferenceForFrame & mnFuseTargetForKF; + // LocalMaping related vars + ar & mnBALocalForKF & mnBAFixedForKF; + // KeyFrameDB related vars + ar & mnLoopQuery & mnLoopWords & mLoopScore & mnRelocQuery & mnRelocWords & mRelocScore; + // LoopClosing related vars + ar & mTcwGBA & mTcwBefGBA & mnBAGlobalForKF; + // calibration parameters + ar & const_cast(fx) & const_cast(fy) & const_cast(cx) & const_cast(cy); + ar & const_cast(invfx) & const_cast(invfy) & const_cast(mbf); + ar & const_cast(mb) & const_cast(mThDepth); + // Number of KeyPoints; + ar & const_cast(N); + // KeyPoints, stereo coordinate and descriptors + ar & const_cast &>(mvKeys); + ar & const_cast &>(mvKeysUn); + ar & const_cast &>(mvuRight); + ar & const_cast &>(mvDepth); + ar & const_cast(mDescriptors); + // Bow + ar & mBowVec & mFeatVec; + // Pose relative to parent + ar & mTcp; + // Scale related + ar & const_cast(mnScaleLevels) & const_cast(mfScaleFactor) & const_cast(mfLogScaleFactor); + ar & const_cast &>(mvScaleFactors) & const_cast &>(mvLevelSigma2) & const_cast &>(mvInvLevelSigma2); + // Image bounds and calibration + ar & const_cast(mnMinX) & const_cast(mnMinY) & const_cast(mnMaxX) & const_cast(mnMaxY); + ar & const_cast(mK); + + // mutex needed vars, but don't lock mutex in the save/load procedure + { + unique_lock lock_pose(mMutexPose); + ar & Tcw & Twc & Ow & Cw; + } + { + unique_lock lock_feature(mMutexFeatures); + ar & mvpMapPoints; // hope boost deal with the pointer graph well + } + // BoW + ar & mpKeyFrameDB; + // mpORBvocabulary restore elsewhere(see SetORBvocab) + { + // Grid related + unique_lock lock_connection(mMutexConnections); + ar & mGrid & mConnectedKeyFrameWeights & mvpOrderedConnectedKeyFrames & mvOrderedWeights; + // Spanning Tree and Loop Edges + ar & mbFirstConnection & mpParent & mspChildrens & mspLoopEdges; + // Bad flags + ar & mbNotErase & mbToBeErased & mbBad & mHalfBaseline; + } + // Map Points + ar & mpMap; + // don't save mutex +} +template void KeyFrame::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void KeyFrame::serialize(boost::archive::binary_oarchive&, const unsigned int); + } //namespace ORB_SLAM diff --git a/orb_slam2/src/KeyFrameDatabase.cc b/orb_slam2/src/KeyFrameDatabase.cc index 826860ca..cc1bec2f 100755 --- a/orb_slam2/src/KeyFrameDatabase.cc +++ b/orb_slam2/src/KeyFrameDatabase.cc @@ -308,4 +308,20 @@ vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F) return vpRelocCandidates; } +// map serialization addition +template +void KeyFrameDatabase::serialize(Archive &ar, const unsigned int version) +{ + // don't save associated vocabulary, KFDB restore by created explicitly from a new ORBvocabulary instance + // inverted file + { + unique_lock lock_InvertedFile(mMutex); + ar & mvInvertedFile; + } + // don't save mutex +} +template void KeyFrameDatabase::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void KeyFrameDatabase::serialize(boost::archive::binary_oarchive&, const unsigned int); + + } //namespace ORB_SLAM diff --git a/orb_slam2/src/LoopClosing.cc b/orb_slam2/src/LoopClosing.cc index cd27b669..eed6c470 100755 --- a/orb_slam2/src/LoopClosing.cc +++ b/orb_slam2/src/LoopClosing.cc @@ -38,7 +38,7 @@ namespace ORB_SLAM2 LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale): mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true), - mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0) + mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(false) { mnCovisibilityConsistencyTh = 3; } @@ -413,7 +413,7 @@ void LoopClosing::CorrectLoop() unique_lock lock(mMutexGBA); mbStopGBA = true; - mnFullBAIdx++; + mnFullBAIdx = true; if(mpThreadGBA) { @@ -646,7 +646,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) { cout << "Starting Global Bundle Adjustment" << endl; - int idx = mnFullBAIdx; + bool idx = mnFullBAIdx; Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false); // Update all MapPoints and KeyFrames diff --git a/orb_slam2/src/Map.cc b/orb_slam2/src/Map.cc index 15fcd869..7e7a0af0 100755 --- a/orb_slam2/src/Map.cc +++ b/orb_slam2/src/Map.cc @@ -130,4 +130,20 @@ void Map::clear() mvpKeyFrameOrigins.clear(); } +// map serialization addition +template +void Map::serialize(Archive &ar, const unsigned int version) +{ + // don't save mutex + unique_lock lock_MapUpdate(mMutexMapUpdate); + unique_lock lock_Map(mMutexMap); + ar & mspMapPoints; + ar & mvpKeyFrameOrigins; + ar & mspKeyFrames; + ar & mvpReferenceMapPoints; + ar & mnMaxKFid & mnBigChangeIdx; +} +template void Map::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void Map::serialize(boost::archive::binary_oarchive&, const unsigned int); + } //namespace ORB_SLAM diff --git a/orb_slam2/src/MapPoint.cc b/orb_slam2/src/MapPoint.cc index 3b292119..cd8089b4 100755 --- a/orb_slam2/src/MapPoint.cc +++ b/orb_slam2/src/MapPoint.cc @@ -416,6 +416,45 @@ int MapPoint::PredictScale(const float ¤tDist, Frame* pF) return nScale; } - +// map serialization addition +MapPoint::MapPoint(): + nObs(0), mnTrackReferenceForFrame(0), + mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0), + mnCorrectedReference(0), mnBAGlobalForKF(0),mnVisible(1), mnFound(1), mbBad(false), + mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0) +{} +template +void MapPoint::serialize(Archive &ar, const unsigned int version) +{ + unique_lock lock_Pos(mMutexPos); + unique_lock lock_Features(mMutexFeatures); + ar & mnId & nNextId & mnFirstKFid & mnFirstFrame & nObs; + // Tracking related vars + ar & mTrackProjX; + ar & mTrackProjY; + ar & mTrackProjXR; + ar & mbTrackInView; + ar & mnTrackScaleLevel; + ar & mTrackViewCos; + ar & mnTrackReferenceForFrame; + ar & mnLastFrameSeen; + // Local Mapping related vars + ar & mnBALocalForKF & mnFuseCandidateForKF; + // Loop Closing related vars + ar & mnLoopPointForKF & mnCorrectedByKF & mnCorrectedReference & mPosGBA & mnBAGlobalForKF; + // don't save the mutex + ar & mWorldPos; + ar & mObservations; + ar & mNormalVector; + ar & mDescriptor; + ar & mpRefKF; + ar & mnVisible & mnFound; + ar & mbBad & mpReplaced; + ar & mfMinDistance & mfMaxDistance; + ar & mpMap; + // don't save the mutex +} +template void MapPoint::serialize(boost::archive::binary_iarchive&, const unsigned int); +template void MapPoint::serialize(boost::archive::binary_oarchive&, const unsigned int); } //namespace ORB_SLAM diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 06b3f15a..aca072de 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -28,9 +28,10 @@ namespace ORB_SLAM2 { -System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor - ):mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), - mbDeactivateLocalizationMode(false) +System::System(const string strVocFile, const eSensor sensor, ORBParameters& parameters, + const std::string & map_file, bool load_map): // map serialization addition + mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), + map_file(map_file), load_map(load_map) { // Output welcome message cout << endl << @@ -39,6 +40,11 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS "This is free software, and you are welcome to redistribute it" << endl << "under certain conditions. See LICENSE.txt." << endl << endl; + cout << "OpenCV version : " << CV_VERSION << endl; + cout << "Major version : " << CV_MAJOR_VERSION << endl; + cout << "Minor version : " << CV_MINOR_VERSION << endl; + cout << "Subminor version : " << CV_SUBMINOR_VERSION << endl; + cout << "Input sensor was set to: "; if(mSensor==MONOCULAR) @@ -48,15 +54,6 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS else if(mSensor==RGBD) cout << "RGB-D" << endl; - //Check settings file - cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); - if(!fsSettings.isOpened()) - { - cerr << "Failed to open settings file at: " << strSettingsFile << endl; - exit(-1); - } - - //Load ORB Vocabulary cout << endl << "Loading ORB Vocabulary." << endl; @@ -68,13 +65,13 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS if(!bVocLoad) { cerr << "Cannot find binary file for vocabulary. " << endl; - cerr << "Falied to open at: " << strVocFile+".bin" << endl; + cerr << "Failed to open at: " << strVocFile+".bin" << endl; cerr << "Trying to open the text file. This could take a while..." << endl; bool bVocLoad2 = mpVocabulary->loadFromTextFile(strVocFile); if(!bVocLoad2) { cerr << "Wrong path to vocabulary. " << endl; - cerr << "Falied to open at: " << strVocFile << endl; + cerr << "Failed to open at: " << strVocFile << endl; exit(-1); } cerr << "Saving the vocabulary to binary for the next time to " << strVocFile+".bin" << endl; @@ -83,11 +80,18 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cout << "Vocabulary loaded!" << endl << endl; - //Create KeyFrame Database - mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); - - //Create the Map - mpMap = new Map(); + // begin map serialization addition + // load serialized map + if (load_map && LoadMap(map_file)) { + std::cout << "Using loaded map with " << mpMap->MapPointsInMap() << " points\n" << std::endl; + } + else { + //Create KeyFrame Database + mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); + //Create the Map + mpMap = new Map(); + } + // end map serialization addition //Create Drawers. These are used by the Viewer mpFrameDrawer = new FrameDrawer(mpMap); @@ -97,7 +101,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, - mpMap, mpDenseMap, mpKeyFrameDatabase, strSettingsFile, mSensor); + mpMap, mpDenseMap, mpKeyFrameDatabase, mSensor, parameters); //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); @@ -287,6 +291,11 @@ bool System::MapChanged() return false; } +bool System::isRunningGBA() +{ + return mpLoopCloser->isRunningGBA(); +} + void System::Reset() { unique_lock lock(mMutexReset); @@ -491,14 +500,59 @@ std::vector System::GetAllMapPoints() { return mpMap->GetAllMapPoints(); } + +bool System::SetCallStackSize (const rlim_t kNewStackSize) { + struct rlimit rlimit; + int operation_result; + + operation_result = getrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Error getting the call stack struct" << std::endl; + return false; + } + + if (kNewStackSize > rlimit.rlim_max) { + std::cerr << "Requested call stack size too large" << std::endl; + return false; + } + + if (rlimit.rlim_cur <= kNewStackSize) { + rlimit.rlim_cur = kNewStackSize; + operation_result = setrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Setrlimit returned result: " << operation_result << std::endl; + return false; + } + return true; + } + return false; +} + + +rlim_t System::GetCurrentCallStackSize () { + struct rlimit rlimit; + int operation_result; + + operation_result = getrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Error getting the call stack struct" << std::endl; + return 16 * 1024L * 1024L; //default + } + + return rlimit.rlim_cur; +} + + void System::ActivateLocalizationMode() { + currently_localizing_only_ = true; unique_lock lock(mMutexMode); mbActivateLocalizationMode = true; } void System::DeactivateLocalizationMode() { + currently_localizing_only_ = false; unique_lock lock(mMutexMode); mbDeactivateLocalizationMode = true; } @@ -512,6 +566,92 @@ void System::EnableLocalizationOnly (bool localize_only) { DeactivateLocalizationMode(); } } + + std::cout << "Enable localization only: " << (localize_only?"true":"false") << std::endl; +} + + +// map serialization addition +bool System::SaveMap(const string &filename) { + unique_lockMapPointGlobal(MapPoint::mGlobalMutex); + std::ofstream out(filename, std::ios_base::binary); + if (!out) { + std::cerr << "cannot write to map file: " << map_file << std::endl; + return false; + } + + const rlim_t kNewStackSize = 64L * 1024L * 1024L; // min stack size = 64 Mb + const rlim_t kDefaultCallStackSize = GetCurrentCallStackSize(); + if (!SetCallStackSize(kNewStackSize)) { + std::cerr << "Error changing the call stack size; Aborting" << std::endl; + return false; + } + + try { + std::cout << "saving map file: " << map_file << std::flush; + boost::archive::binary_oarchive oa(out, boost::archive::no_header); + oa << mpMap; + oa << mpKeyFrameDatabase; + std::cout << " ... done" << std::endl; + out.close(); + } catch (const std::exception &e) { + std::cerr << e.what() << std::endl; + SetCallStackSize(kDefaultCallStackSize); + return false; + } catch (...) { + std::cerr << "Unknows exeption" << std::endl; + SetCallStackSize(kDefaultCallStackSize); + return false; + } + + SetCallStackSize(kDefaultCallStackSize); + return true; +} + +bool System::LoadMap(const string &filename) { + + unique_lockMapPointGlobal(MapPoint::mGlobalMutex); + std::ifstream in(filename, std::ios_base::binary); + if (!in) { + cerr << "Cannot open map file: " << map_file << " , you need create it first!" << std::endl; + return false; + } + + const rlim_t kNewStackSize = 64L * 1024L * 1024L; // min stack size = 64 Mb + const rlim_t kDefaultCallStackSize = GetCurrentCallStackSize(); + if (!SetCallStackSize(kNewStackSize)) { + std::cerr << "Error changing the call stack size; Aborting" << std::endl; + return false; + } + + std::cout << "Loading map file: " << map_file << std::flush; + boost::archive::binary_iarchive ia(in, boost::archive::no_header); + ia >> mpMap; + ia >> mpKeyFrameDatabase; + mpKeyFrameDatabase->SetORBvocabulary(mpVocabulary); + std::cout << " ... done" << std::endl; + + std::cout << "Map reconstructing" << flush; + vector vpKFS = mpMap->GetAllKeyFrames(); + unsigned long mnFrameId = 0; + for (auto it:vpKFS) { + + it->SetORBvocabulary(mpVocabulary); + it->ComputeBoW(); + + if (it->mnFrameId > mnFrameId) { + mnFrameId = it->mnFrameId; + } + } + + Frame::nNextId = mnFrameId; + + std::cout << " ... done" << std::endl; + in.close(); + + SetCallStackSize(kDefaultCallStackSize); + + return true; } } //namespace ORB_SLAM diff --git a/orb_slam2/src/Tracking.cc b/orb_slam2/src/Tracking.cc index b05ab7e9..40daf974 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -43,79 +43,66 @@ using namespace std; namespace ORB_SLAM2 { -Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, Map *pMap, DenseMap* dense_map, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): +Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, + Map *pMap, DenseMap* dense_map, KeyFrameDatabase* pKFDB, const int sensor, ORBParameters& parameters): mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpFrameDrawer(pFrameDrawer), mpMap(pMap), dense_map_ (dense_map), mnLastRelocFrameId(0), mnMinimumKeyFrames(5) { - // Load camera parameters from settings file - - cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); - float fx = fSettings["Camera.fx"]; - float fy = fSettings["Camera.fy"]; - float cx = fSettings["Camera.cx"]; - float cy = fSettings["Camera.cy"]; + //Unpack all the parameters from the parameters struct (this replaces loading in a second configuration file) + mMaxFrames = parameters.maxFrames; + mbRGB = parameters.RGB; + mThDepth = parameters.thDepth; + nFeatures = parameters.nFeatures; + fScaleFactor = parameters.scaleFactor; + nLevels = parameters.nLevels; + fIniThFAST = parameters.iniThFAST; + fMinThFAST = parameters.minThFAST; + mDepthMapFactor = parameters.depthMapFactor; cv::Mat K = cv::Mat::eye(3,3,CV_32F); - K.at(0,0) = fx; - K.at(1,1) = fy; - K.at(0,2) = cx; - K.at(1,2) = cy; + K.at(0,0) = parameters.fx; + K.at(1,1) = parameters.fy; + K.at(0,2) = parameters.cx; + K.at(1,2) = parameters.cy; K.copyTo(mK); cv::Mat DistCoef(4,1,CV_32F); - DistCoef.at(0) = fSettings["Camera.k1"]; - DistCoef.at(1) = fSettings["Camera.k2"]; - DistCoef.at(2) = fSettings["Camera.p1"]; - DistCoef.at(3) = fSettings["Camera.p2"]; - const float k3 = fSettings["Camera.k3"]; - if(k3!=0) + DistCoef.at(0) = parameters.k1; + DistCoef.at(1) = parameters.k2; + DistCoef.at(2) = parameters.p1; + DistCoef.at(3) = parameters.p2; + if(parameters.k3!=0) { DistCoef.resize(5); - DistCoef.at(4) = k3; + DistCoef.at(4) = parameters.k3; } DistCoef.copyTo(mDistCoef); - mbf = fSettings["Camera.bf"]; + mbf = parameters.baseline; - float fps = fSettings["Camera.fps"]; - if(fps==0) - fps=30; - - // Max/Min Frames to insert keyframes and to check relocalisation + // Max/Min Frames to insert keyframes and to check relocalization mMinFrames = 0; - mMaxFrames = fps; cout << endl << "Camera Parameters: " << endl; - cout << "- fx: " << fx << endl; - cout << "- fy: " << fy << endl; - cout << "- cx: " << cx << endl; - cout << "- cy: " << cy << endl; + cout << "- fx: " << parameters.fx << endl; + cout << "- fy: " << parameters.fy << endl; + cout << "- cx: " << parameters.cx << endl; + cout << "- cy: " << parameters.cy << endl; cout << "- k1: " << DistCoef.at(0) << endl; cout << "- k2: " << DistCoef.at(1) << endl; if(DistCoef.rows==5) cout << "- k3: " << DistCoef.at(4) << endl; cout << "- p1: " << DistCoef.at(2) << endl; cout << "- p2: " << DistCoef.at(3) << endl; - cout << "- fps: " << fps << endl; - - - int nRGB = fSettings["Camera.RGB"]; - mbRGB = nRGB; + cout << "- fps: " << mMaxFrames << endl; + cout << "- bf: " << mbf << endl; if(mbRGB) cout << "- color order: RGB (ignored if grayscale)" << endl; else cout << "- color order: BGR (ignored if grayscale)" << endl; - // Load ORB parameters - - int nFeatures = fSettings["ORBextractor.nFeatures"]; - float fScaleFactor = fSettings["ORBextractor.scaleFactor"]; - int nLevels = fSettings["ORBextractor.nLevels"]; - int fIniThFAST = fSettings["ORBextractor.iniThFAST"]; - int fMinThFAST = fSettings["ORBextractor.minThFAST"]; - mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); if(sensor==System::STEREO) @@ -133,13 +120,12 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, if(sensor==System::STEREO || sensor==System::RGBD) { - mThDepth = mbf*(float)fSettings["ThDepth"]/fx; + mThDepth = mbf*(float)mThDepth/parameters.fx; cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl; } if(sensor==System::RGBD) { - mDepthMapFactor = fSettings["DepthMapFactor"]; if(fabs(mDepthMapFactor)<1e-5) mDepthMapFactor=1; else @@ -1488,6 +1474,7 @@ bool Tracking::Relocalization() if(!bMatch) { + mCurrentFrame.mTcw = cv::Mat::zeros(0, 0, CV_32F); // this prevents a segfault later (https://github.com/raulmur/ORB_SLAM2/pull/381#issuecomment-337312336) return false; } else diff --git a/package.xml b/package.xml index e2b5ca62..aa1dd9b9 100644 --- a/package.xml +++ b/package.xml @@ -9,21 +9,30 @@ GPLv3 catkin + roscpp + rospy std_msgs cv_bridge image_transport tf + tf2_geometry_msgs + tf2_ros sensor_msgs libpcl-all-dev + dynamic_reconfigure + message_generation + roscpp + rospy std_msgs cv_bridge image_transport tf sensor_msgs libpcl-all - + dynamic_reconfigure + message_runtime diff --git a/ros/config/dynamic_reconfigure.cfg b/ros/config/dynamic_reconfigure.cfg new file mode 100755 index 00000000..c8365689 --- /dev/null +++ b/ros/config/dynamic_reconfigure.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python +PACKAGE = "orb_slam2_ros" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("localize_only", bool_t, 0, "Disable mapping, localize only", False) +gen.add("reset_map", bool_t, 0, "Erase the map; toggles back to false automatically", False) +gen.add("min_num_kf_in_map", int_t, 0, "Minimum number of key frames in the map for initialization", 5, 1, 50) +gen.add("min_observations_for_ros_map", int_t, 0, "Min num of observations per point for the point cloud; Doesn't change the SLAM itself", 4, 2, 30) + +exit(gen.generate(PACKAGE, "orb_slam2_ros", "dynamic_reconfigure")) diff --git a/ros/config/rviz_config.rviz b/ros/config/rviz_config.rviz new file mode 100644 index 00000000..ae040796 --- /dev/null +++ b/ros/config/rviz_config.rviz @@ -0,0 +1,197 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 348 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /orb_slam2_rgbd/debug_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Flat Squares + Topic: /orb_slam2_rgbd/map_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.300000012 + Head Radius: 0.100000001 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.0500000007 + Shape: Arrow + Topic: /orb_slam2_rgbd/pose + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /orb_slam2_rgbd/trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 24.7596321 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.785398006 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002adfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001eb000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000230000000bc0000001600ffffff000000010000010f000002adfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000002ad000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e70000003efc0100000002fb0000000800540069006d00650100000000000005e7000002ff00fffffffb0000000800540069006d0065010000000000000450000000000000000000000477000002ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1511 + X: 2199 + Y: 34 diff --git a/ros/include/MonoNode.h b/ros/include/MonoNode.h index 5268f80a..13218908 100644 --- a/ros/include/MonoNode.h +++ b/ros/include/MonoNode.h @@ -40,7 +40,7 @@ class MonoNode : public Node { public: - MonoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); + MonoNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); ~MonoNode (); void ImageCallback (const sensor_msgs::ImageConstPtr& msg); diff --git a/ros/include/Node.h b/ros/include/Node.h index 238e64c6..594d6878 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -25,15 +25,27 @@ #include #include #include -#include #include #include +#include +#include +#include +#include + +#include +#include + +#include "orb_slam2_ros/SaveMap.h" + #include #include #include #include #include +#include +#include +#include #include "System.h" @@ -42,36 +54,60 @@ class Node { public: - Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); + Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); ~Node (); + void Init (); protected: void Update (); ORB_SLAM2::System* orb_slam_; - ros::Time current_frame_time_; + std::string camera_info_topic_; + private: void PublishMapPoints (std::vector map_points); void PublishPositionAsTransform (cv::Mat position); + void PublishPositionAsPoseStamped(cv::Mat position); + void PublishGBAStatus (bool gba_status); void PublishRenderedImage (cv::Mat image); - void UpdateParameters (); - tf::Transform TransformFromMat (cv::Mat position_mat); + void ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level); + bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res); + void LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters); + + // initialization Transform listener + boost::shared_ptr tfBuffer; + boost::shared_ptr tfListener; + + tf2::Transform TransformFromMat (cv::Mat position_mat); + tf2::Transform TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target); sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector map_points); - void InitParameters (); + + dynamic_reconfigure::Server dynamic_param_server_; image_transport::Publisher rendered_image_publisher_; ros::Publisher map_points_publisher_; + ros::Publisher pose_publisher_; + ros::Publisher status_gba_publisher_; + + ros::ServiceServer service_server_; std::string name_of_node_; ros::NodeHandle node_handle_; + image_transport::ImageTransport image_transport_; + + ORB_SLAM2::System::eSensor sensor_; - bool localize_only_param_; - bool reset_map_param_; std::string map_frame_id_param_; std::string camera_frame_id_param_; - int minimum_num_of_kf_in_map_param_; + std::string target_frame_id_param_; + std::string map_file_name_param_; + std::string voc_file_name_param_; + bool load_map_param_; bool publish_pointcloud_param_; + bool publish_tf_param_; + bool publish_pose_param_; + int min_observations_per_point_; }; #endif //ORBSLAM2_ROS_NODE_H_ diff --git a/ros/include/RGBDNode.h b/ros/include/RGBDNode.h index cacdd55e..4df5c9be 100644 --- a/ros/include/RGBDNode.h +++ b/ros/include/RGBDNode.h @@ -43,7 +43,7 @@ class RGBDNode : public Node { public: - RGBDNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); + RGBDNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); ~RGBDNode (); void ImageCallback (const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); diff --git a/ros/include/StereoNode.h b/ros/include/StereoNode.h index fed7c7cf..b54be2a2 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -4,8 +4,13 @@ #include #include #include +#include +#include #include +#include +#include +#include #include #include #include @@ -18,9 +23,17 @@ class StereoNode : public Node { public: - StereoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); + StereoNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); ~StereoNode (); void ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight); +private: + typedef message_filters::sync_policies::ApproximateTime sync_pol; + message_filters::Subscriber *left_sub_; + message_filters::Subscriber *right_sub_; + message_filters::Synchronizer *sync_; + + int resize_horizontal; + int resize_vertical; }; diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch new file mode 100644 index 00000000..a83cc1df --- /dev/null +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch new file mode 100644 index 00000000..401127af --- /dev/null +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_mono.launch b/ros/launch/orb_slam2_mono.launch deleted file mode 100644 index 2446c3ad..00000000 --- a/ros/launch/orb_slam2_mono.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch new file mode 100644 index 00000000..c2c65f5c --- /dev/null +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch new file mode 100644 index 00000000..0b8a43bd --- /dev/null +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch new file mode 100644 index 00000000..26fa7440 --- /dev/null +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch new file mode 100644 index 00000000..6ed07ff9 --- /dev/null +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch new file mode 100644 index 00000000..de979b11 --- /dev/null +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_rgbd.launch b/ros/launch/orb_slam2_rgbd.launch deleted file mode 100644 index 19109c4b..00000000 --- a/ros/launch/orb_slam2_rgbd.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/ros/launch/orb_slam2_stereo.launch b/ros/launch/orb_slam2_stereo.launch deleted file mode 100644 index 9e2649d1..00000000 --- a/ros/launch/orb_slam2_stereo.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/ros/launch/orb_slam2_tum2_rgbd.launch b/ros/launch/orb_slam2_tum2_rgbd.launch new file mode 100644 index 00000000..b5a9ef14 --- /dev/null +++ b/ros/launch/orb_slam2_tum2_rgbd.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_zed2_stereo.launch b/ros/launch/orb_slam2_zed2_stereo.launch new file mode 100644 index 00000000..0e7a22cd --- /dev/null +++ b/ros/launch/orb_slam2_zed2_stereo.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc index d573f0d4..bfb03b38 100644 --- a/ros/src/MonoNode.cc +++ b/ros/src/MonoNode.cc @@ -1,31 +1,25 @@ #include "MonoNode.h" +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "Mono"); ros::start(); - if(argc != 3) - { - ROS_ERROR ("Path to vocabulary and path to settings need to be set."); - ros::shutdown(); - return 1; + if(argc > 1) { + ROS_WARN ("Arguments supplied via command line are neglected."); } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR); ros::NodeHandle node_handle; image_transport::ImageTransport image_transport (node_handle); - MonoNode node (&SLAM, node_handle, image_transport); - - ros::spin(); + MonoNode node (ORB_SLAM2::System::MONOCULAR, node_handle, image_transport); - // Stop all threads - SLAM.Shutdown(); + node.Init(); - // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + ros::spin(); ros::shutdown(); @@ -33,8 +27,9 @@ int main(int argc, char **argv) } -MonoNode::MonoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { +MonoNode::MonoNode (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (sensor, node_handle, image_transport) { image_subscriber = image_transport.subscribe ("/camera/image_raw", 1, &MonoNode::ImageCallback, this); + camera_info_topic_ = "/camera/camera_info"; } diff --git a/ros/src/Node.cc b/ros/src/Node.cc index a136c425..a2ad08bc 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -2,22 +2,64 @@ #include -Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) { +Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : image_transport_(image_transport) { name_of_node_ = ros::this_node::getName(); - orb_slam_ = pSLAM; node_handle_ = node_handle; + min_observations_per_point_ = 2; + sensor_ = sensor; +} + + +Node::~Node () { + // Stop all threads + orb_slam_->Shutdown(); + + // Save camera trajectory + orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + delete orb_slam_; +} + +void Node::Init () { + //static parameters + node_handle_.param(name_of_node_+ "/publish_pointcloud", publish_pointcloud_param_, true); + node_handle_.param(name_of_node_+ "/publish_pose", publish_pose_param_, true); + node_handle_.param(name_of_node_+ "/publish_tf", publish_tf_param_, true); + node_handle_.param(name_of_node_+ "/pointcloud_frame_id", map_frame_id_param_, "map"); + node_handle_.param(name_of_node_+ "/camera_frame_id", camera_frame_id_param_, "camera_link"); + node_handle_.param(name_of_node_+ "/target_frame_id", target_frame_id_param_, "base_link"); + node_handle_.param(name_of_node_ + "/map_file", map_file_name_param_, "map.bin"); + node_handle_.param(name_of_node_ + "/voc_file", voc_file_name_param_, "file_not_set"); + node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); + + // Create a parameters object to pass to the Tracking system + ORB_SLAM2::ORBParameters parameters; + LoadOrbParameters (parameters); + + orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor_, parameters, map_file_name_param_, load_map_param_); + + service_server_ = node_handle_.advertiseService(name_of_node_+"/save_map", &Node::SaveMapSrv, this); + + //Setup dynamic reconfigure + dynamic_reconfigure::Server::CallbackType dynamic_param_callback; + dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2); + dynamic_param_server_.setCallback(dynamic_param_callback); - InitParameters (); + // Initialization transformation listener + tfBuffer.reset(new tf2_ros::Buffer); + tfListener.reset(new tf2_ros::TransformListener(*tfBuffer)); - rendered_image_publisher_ = image_transport.advertise (name_of_node_+"/debug_image", 1); + rendered_image_publisher_ = image_transport_.advertise (name_of_node_+"/debug_image", 1); if (publish_pointcloud_param_) { map_points_publisher_ = node_handle_.advertise (name_of_node_+"/map_points", 1); } -} - -Node::~Node () { + // Enable publishing camera's pose as PoseStamped message + if (publish_pose_param_) { + pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); + } + status_gba_publisher_ = node_handle_.advertise (name_of_node_+"/gba_running", 1); } @@ -25,7 +67,13 @@ void Node::Update () { cv::Mat position = orb_slam_->GetCurrentPosition(); if (!position.empty()) { - PublishPositionAsTransform (position); + if (publish_tf_param_){ + PublishPositionAsTransform(position); + } + + if (publish_pose_param_) { + PublishPositionAsPoseStamped(position); + } } PublishRenderedImage (orb_slam_->DrawCurrentFrame()); @@ -34,7 +82,8 @@ void Node::Update () { PublishMapPoints (orb_slam_->GetAllMapPoints()); } - UpdateParameters (); + PublishGBAStatus (orb_slam_->isRunningGBA()); + } @@ -43,13 +92,97 @@ void Node::PublishMapPoints (std::vector map_points) { map_points_publisher_.publish (cloud); } +tf2::Transform Node::TransformToTarget (tf2::Transform tf_in, std::string frame_in, std::string frame_target) { + // Transform tf_in from frame_in to frame_target + tf2::Transform tf_map2orig = tf_in; + tf2::Transform tf_orig2target; + tf2::Transform tf_map2target; + + tf2::Stamped transformStamped_temp; + try { + // Get the transform from camera to target + geometry_msgs::TransformStamped tf_msg = tfBuffer->lookupTransform(frame_in, frame_target, ros::Time(0)); + // Convert to tf2 + tf2::fromMsg(tf_msg, transformStamped_temp); + tf_orig2target.setBasis(transformStamped_temp.getBasis()); + tf_orig2target.setOrigin(transformStamped_temp.getOrigin()); + + } catch (tf2::TransformException &ex) { + ROS_WARN("%s",ex.what()); + //ros::Duration(1.0).sleep(); + tf_orig2target.setIdentity(); + } + + /* + // Print debug info + double roll, pitch, yaw; + // Print debug map2orig + tf2::Matrix3x3(tf_map2orig.getRotation()).getRPY(roll, pitch, yaw); + ROS_INFO("Static transform Map to Orig [%s -> %s]", + map_frame_id_param_.c_str(), frame_in.c_str()); + ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}", + tf_map2orig.getOrigin().x(), tf_map2orig.getOrigin().y(), tf_map2orig.getOrigin().z()); + ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}", + RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw)); + // Print debug tf_orig2target + tf2::Matrix3x3(tf_orig2target.getRotation()).getRPY(roll, pitch, yaw); + ROS_INFO("Static transform Orig to Target [%s -> %s]", + frame_in.c_str(), frame_target.c_str()); + ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}", + tf_orig2target.getOrigin().x(), tf_orig2target.getOrigin().y(), tf_orig2target.getOrigin().z()); + ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}", + RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw)); + // Print debug map2target + tf2::Matrix3x3(tf_map2target.getRotation()).getRPY(roll, pitch, yaw); + ROS_INFO("Static transform Map to Target [%s -> %s]", + map_frame_id_param_.c_str(), frame_target.c_str()); + ROS_INFO(" * Translation: {%.3f,%.3f,%.3f}", + tf_map2target.getOrigin().x(), tf_map2target.getOrigin().y(), tf_map2target.getOrigin().z()); + ROS_INFO(" * Rotation: {%.3f,%.3f,%.3f}", + RAD2DEG(roll), RAD2DEG(pitch), RAD2DEG(yaw)); + */ + + // Transform from map to target + tf_map2target = tf_map2orig * tf_orig2target; + return tf_map2target; +} void Node::PublishPositionAsTransform (cv::Mat position) { - tf::Transform transform = TransformFromMat (position); - static tf::TransformBroadcaster tf_broadcaster; - tf_broadcaster.sendTransform(tf::StampedTransform(transform, current_frame_time_, map_frame_id_param_, camera_frame_id_param_)); + // Get transform from map to camera frame + tf2::Transform tf_transform = TransformFromMat(position); + + // Make transform from camera frame to target frame + tf2::Transform tf_map2target = TransformToTarget(tf_transform, camera_frame_id_param_, target_frame_id_param_); + + // Make message + tf2::Stamped tf_map2target_stamped; + tf_map2target_stamped = tf2::Stamped(tf_map2target, current_frame_time_, map_frame_id_param_); + geometry_msgs::TransformStamped msg = tf2::toMsg(tf_map2target_stamped); + msg.child_frame_id = target_frame_id_param_; + // Broadcast tf + static tf2_ros::TransformBroadcaster tf_broadcaster; + tf_broadcaster.sendTransform(msg); } +void Node::PublishPositionAsPoseStamped (cv::Mat position) { + tf2::Transform tf_position = TransformFromMat(position); + + // Make transform from camera frame to target frame + tf2::Transform tf_position_target = TransformToTarget(tf_position, camera_frame_id_param_, target_frame_id_param_); + + // Make message + tf2::Stamped tf_position_target_stamped; + tf_position_target_stamped = tf2::Stamped(tf_position_target, current_frame_time_, map_frame_id_param_); + geometry_msgs::PoseStamped pose_msg; + tf2::toMsg(tf_position_target_stamped, pose_msg); + pose_publisher_.publish(pose_msg); +} + +void Node::PublishGBAStatus (bool gba_status) { + std_msgs::Bool gba_status_msg; + gba_status_msg.data = gba_status; + status_gba_publisher_.publish(gba_status_msg); +} void Node::PublishRenderedImage (cv::Mat image) { std_msgs::Header header; @@ -60,45 +193,39 @@ void Node::PublishRenderedImage (cv::Mat image) { } -tf::Transform Node::TransformFromMat (cv::Mat position_mat) { +tf2::Transform Node::TransformFromMat (cv::Mat position_mat) { cv::Mat rotation(3,3,CV_32F); cv::Mat translation(3,1,CV_32F); - rotation = position_mat.rowRange(0,3).colRange(0,3).t(); - translation = rotation*position_mat.rowRange(0,3).col(3); + rotation = position_mat.rowRange(0,3).colRange(0,3); + translation = position_mat.rowRange(0,3).col(3); + - tf::Matrix3x3 tf_camera_rotation (rotation.at (0,0), rotation.at (0,1), rotation.at (0,2), + tf2::Matrix3x3 tf_camera_rotation (rotation.at (0,0), rotation.at (0,1), rotation.at (0,2), rotation.at (1,0), rotation.at (1,1), rotation.at (1,2), rotation.at (2,0), rotation.at (2,1), rotation.at (2,2) ); - tf::Vector3 tf_camera_translation (translation.at (0), translation.at (1), translation.at (2)); + tf2::Vector3 tf_camera_translation (translation.at (0), translation.at (1), translation.at (2)); - const tf::Matrix3x3 Rx (1, 0, 0, - 0, 0, -1, - 0, 1, 0); + //Coordinate transformation matrix from orb coordinate system to ros coordinate system + const tf2::Matrix3x3 tf_orb_to_ros (0, 0, 1, + -1, 0, 0, + 0,-1, 0); - const tf::Matrix3x3 Rz (0, -1, 0, - 1, 0, 0, - 0, 0, 1); + //Transform from orb coordinate system to ros coordinate system on camera coordinates + tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation; + tf_camera_translation = tf_orb_to_ros*tf_camera_translation; - const tf::Matrix3x3 invX (-1, 0, 0, - 0, 1, 0, - 0, 0, 1); + //Inverse matrix + tf_camera_rotation = tf_camera_rotation.transpose(); + tf_camera_translation = -(tf_camera_rotation*tf_camera_translation); - const tf::Matrix3x3 invYZ (1, 0, 0, - 0, -1, 0, - 0, 0, -1); + //Transform from orb coordinate system to ros coordinate system on map coordinates + tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation; + tf_camera_translation = tf_orb_to_ros*tf_camera_translation; - tf_camera_rotation = Rx*tf_camera_rotation; - tf_camera_rotation = Rz*tf_camera_rotation; - tf_camera_translation = Rx*tf_camera_translation; - tf_camera_translation = Rz*tf_camera_translation; - - tf_camera_rotation = invYZ*tf_camera_rotation; - tf_camera_translation = invX*tf_camera_translation; - - return tf::Transform (tf_camera_rotation, tf_camera_translation); + return tf2::Transform (tf_camera_rotation, tf_camera_translation); } @@ -133,15 +260,15 @@ sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vectorisBad()) { + if (map_points.at(i)->nObs >= min_observations_per_point_) { data_array[0] = map_points.at(i)->GetWorldPos().at (2); //x. Do the transformation by just reading at the position of z instead of x data_array[1] = -1.0* map_points.at(i)->GetWorldPos().at (0); //y. Do the transformation by just reading at the position of x instead of y data_array[2] = -1.0* map_points.at(i)->GetWorldPos().at (1); //z. Do the transformation by just reading at the position of y instead of z //TODO dont hack the transformation but have a central conversion function for MapPointsToPointCloud and TransformFromMat - memcpy(cloud_data_ptr+(i*cloud.point_step), data_array, 3*sizeof(float)); + memcpy(cloud_data_ptr+(i*cloud.point_step), data_array, num_channels*sizeof(float)); } } @@ -149,27 +276,89 @@ sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vector(name_of_node_+"/pointcloud_frame_id", map_frame_id_param_, "map"); - node_handle_.param(name_of_node_+"/camera_frame_id", camera_frame_id_param_, "camera_link"); - node_handle_.param(name_of_node_+"/min_num_kf_in_map", minimum_num_of_kf_in_map_param_, 5); - orb_slam_->SetMinimumKeyFrames (minimum_num_of_kf_in_map_param_); +void Node::ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level) { + orb_slam_->EnableLocalizationOnly (config.localize_only); + min_observations_per_point_ = config.min_observations_for_ros_map; + + if (config.reset_map) { + orb_slam_->Reset(); + config.reset_map = false; + } + + orb_slam_->SetMinimumKeyFrames (config.min_num_kf_in_map); } -void Node::UpdateParameters () { - node_handle_.param(name_of_node_+"/localize_only", localize_only_param_, false); - orb_slam_->EnableLocalizationOnly (localize_only_param_); +bool Node::SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res) { + res.success = orb_slam_->SaveMap(req.name); - node_handle_.param(name_of_node_+"/reset_map", reset_map_param_, false); - if (reset_map_param_) { - orb_slam_->Reset(); - node_handle_.setParam (name_of_node_+"/reset_map", false); + if (res.success) { + ROS_INFO_STREAM ("Map was saved as " << req.name); + } else { + ROS_ERROR ("Map could not be saved."); } - node_handle_.param(name_of_node_+"/min_num_kf_in_map", minimum_num_of_kf_in_map_param_, 5); - orb_slam_->SetMinimumKeyFrames (minimum_num_of_kf_in_map_param_); + return res.success; +} + + +void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { + //ORB SLAM configuration parameters + node_handle_.param(name_of_node_ + "/camera_fps", parameters.maxFrames, 30); + node_handle_.param(name_of_node_ + "/camera_rgb_encoding", parameters.RGB, true); + node_handle_.param(name_of_node_ + "/ORBextractor/nFeatures", parameters.nFeatures, 1200); + node_handle_.param(name_of_node_ + "/ORBextractor/scaleFactor", parameters.scaleFactor, static_cast(1.2)); + node_handle_.param(name_of_node_ + "/ORBextractor/nLevels", parameters.nLevels, 8); + node_handle_.param(name_of_node_ + "/ORBextractor/iniThFAST", parameters.iniThFAST, 20); + node_handle_.param(name_of_node_ + "/ORBextractor/minThFAST", parameters.minThFAST, 7); + + bool load_calibration_from_cam = false; + node_handle_.param(name_of_node_ + "/load_calibration_from_cam", load_calibration_from_cam, false); + + if (sensor_== ORB_SLAM2::System::STEREO || sensor_==ORB_SLAM2::System::RGBD) { + node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); + node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); + } + + if (load_calibration_from_cam) { + ROS_INFO_STREAM ("Listening for camera info on topic " << node_handle_.resolveName(camera_info_topic_)); + sensor_msgs::CameraInfo::ConstPtr camera_info = ros::topic::waitForMessage(camera_info_topic_, ros::Duration(1000.0)); + if(camera_info == nullptr){ + ROS_WARN("Did not receive camera info before timeout, defaulting to launch file params."); + } else { + parameters.fx = camera_info->K[0]; + parameters.fy = camera_info->K[4]; + parameters.cx = camera_info->K[2]; + parameters.cy = camera_info->K[5]; + + parameters.baseline = camera_info->P[3]; + + parameters.k1 = camera_info->D[0]; + parameters.k2 = camera_info->D[1]; + parameters.p1 = camera_info->D[2]; + parameters.p2 = camera_info->D[3]; + parameters.k3 = camera_info->D[4]; + return; + } + } + + bool got_cam_calibration = true; + if (sensor_== ORB_SLAM2::System::STEREO || sensor_==ORB_SLAM2::System::RGBD) { + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_baseline", parameters.baseline); + } + + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_fx", parameters.fx); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_fy", parameters.fy); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_cx", parameters.cx); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_cy", parameters.cy); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_k1", parameters.k1); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_k2", parameters.k2); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_p1", parameters.p1); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_p2", parameters.p2); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_k3", parameters.k3); + + if (!got_cam_calibration) { + ROS_ERROR ("Failed to get camera calibration parameters from the launch file."); + throw std::runtime_error("No cam calibration"); + } } diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc index 796766fb..49f77ed0 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -1,31 +1,26 @@ #include "RGBDNode.h" +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "RGBD"); ros::start(); - if(argc != 3) - { - ROS_ERROR ("Path to vocabulary and path to settings need to be set."); - ros::shutdown(); - return 1; + if(argc > 1) { + ROS_WARN ("Arguments supplied via command line are neglected."); } - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD); ros::NodeHandle node_handle; - image_transport::ImageTransport image_transport (node_handle); - RGBDNode node (&SLAM, node_handle, image_transport); + // Create SLAM system. It initializes all system threads and gets ready to process frames. + image_transport::ImageTransport image_transport (node_handle); - ros::spin(); + RGBDNode node (ORB_SLAM2::System::RGBD, node_handle, image_transport); - // Stop all threads - SLAM.Shutdown(); + node.Init(); - // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + ros::spin(); ros::shutdown(); @@ -33,9 +28,10 @@ int main(int argc, char **argv) } -RGBDNode::RGBDNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { +RGBDNode::RGBDNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (sensor, node_handle, image_transport) { rgb_subscriber_ = new message_filters::Subscriber (node_handle, "/camera/rgb/image_raw", 1); depth_subscriber_ = new message_filters::Subscriber (node_handle, "/camera/depth_registered/image_raw", 1); + camera_info_topic_ = "/camera/rgb/camera_info"; sync_ = new message_filters::Synchronizer (sync_pol(10), *rgb_subscriber_, *depth_subscriber_); sync_->registerCallback(boost::bind(&RGBDNode::ImageCallback, this, _1, _2)); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index ee8bb0d4..988350a8 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -4,48 +4,45 @@ #include #include +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "Stereo"); ros::start(); - if(argc != 3) - { - ROS_ERROR ("Path to vocabulary and path to settings need to be set."); - ros::shutdown(); - return 1; + if(argc > 1) { + ROS_WARN ("Arguments supplied via command line are neglected."); } - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO); ros::NodeHandle node_handle; image_transport::ImageTransport image_transport (node_handle); - message_filters::Subscriber left_sub(node_handle, "image_left/image_color_rect", 1); - message_filters::Subscriber right_sub(node_handle, "image_right/image_color_rect", 1); - typedef message_filters::sync_policies::ApproximateTime sync_pol; - message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); - - // initilaize - StereoNode node (&SLAM, node_handle, image_transport); + // initialize + StereoNode node (ORB_SLAM2::System::STEREO, node_handle, image_transport); - // register callbacks - sync.registerCallback(boost::bind(&StereoNode::ImageCallback, &node,_1,_2)); + node.Init(); ros::spin(); - // Stop all threads - SLAM.Shutdown(); - return 0; } -StereoNode::StereoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, node_handle, image_transport) { +StereoNode::StereoNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (sensor, node_handle, image_transport) { + left_sub_ = new message_filters::Subscriber (node_handle, "image_left/image_color_rect", 1); + right_sub_ = new message_filters::Subscriber (node_handle, "image_right/image_color_rect", 1); + camera_info_topic_ = "image_left/camera_info"; + + sync_ = new message_filters::Synchronizer (sync_pol(10), *left_sub_, *right_sub_); + sync_->registerCallback(boost::bind(&StereoNode::ImageCallback, this, _1, _2)); } StereoNode::~StereoNode () { + delete left_sub_; + delete right_sub_; + delete sync_; } @@ -68,7 +65,7 @@ void StereoNode::ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft, const current_frame_time_ = msgLeft->header.stamp; - orb_slam_->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); + orb_slam_->TrackStereo(cv_ptrLeft->image, cv_ptrRight->image, cv_ptrLeft->header.stamp.toSec()); Update (); } diff --git a/srv/SaveMap.srv b/srv/SaveMap.srv new file mode 100644 index 00000000..dc5ebb75 --- /dev/null +++ b/srv/SaveMap.srv @@ -0,0 +1,3 @@ +string name +--- +bool success \ No newline at end of file