From bd2efdee687790a98eab43dbfd10b3c241cbc56d Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 9 Feb 2019 18:22:58 +0100 Subject: [PATCH 01/88] Ignore the meta files from CLion --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index d8bb6431..b7c2a180 100755 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ CMakeLists.txt.user +.idea/ KeyFrameTrajectory.txt Thirdparty/DBoW2/build/ Thirdparty/DBoW2/lib/ From cfb82f647d20e0ff491aa1b064aaf1eee12c5d5a Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 9 Feb 2019 19:19:30 +0100 Subject: [PATCH 02/88] Fix using an int variable as a bool --- orb_slam2/src/LoopClosing.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 From 3c6c96e480f161ed3df85464ae45ad6d120739d2 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 9 Feb 2019 19:20:13 +0100 Subject: [PATCH 03/88] Fix the C++11 compiler warning --- CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ae84889e..3ecaefe3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,9 +10,7 @@ ENDIF() # 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) -ELSE() +IF(!COMPILER_SUPPORTS_CXX11) MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") ENDIF() From a76e04661df41cfee54aae6b05118e032d0cabbe Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sun, 10 Feb 2019 13:29:08 +0100 Subject: [PATCH 04/88] Add the dynamic_reconfigure package as a dependency; Add a sample param.cfg file --- CMakeLists.txt | 10 +++++++++- package.xml | 5 ++++- ros/config/dynamic_reconfigure.cfg | 21 +++++++++++++++++++++ 3 files changed, 34 insertions(+), 2 deletions(-) create mode 100755 ros/config/dynamic_reconfigure.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ecaefe3..5c05cc3b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,11 +18,13 @@ 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 sensor_msgs +dynamic_reconfigure ) find_package(OpenCV 3.0 QUIET) @@ -35,6 +37,12 @@ endif() find_package(Eigen3 3.1.0 REQUIRED) +execute_process(COMMAND chmod a+x ros/config/dynamic_reconfigure.cfg + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE cmd_result + OUTPUT_VARIABLE cmd_ver) +message(STATUS "Chmod a+x the dynamic_reconfigure file") + set(LIBS_ORBSLAM ${OpenCV_LIBS} ${EIGEN3_LIBS} @@ -49,7 +57,7 @@ ${catkin_LIBRARIES} ) 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 LIBRARIES {PROJECT_NAME} libDBoW2 libg2o ) diff --git a/package.xml b/package.xml index 2367df32..fc353331 100644 --- a/package.xml +++ b/package.xml @@ -10,18 +10,21 @@ catkin roscpp + rospy std_msgs cv_bridge image_transport tf sensor_msgs + dynamic_reconfigure roscpp + rospy std_msgs cv_bridge image_transport tf sensor_msgs - + dynamic_reconfigure diff --git a/ros/config/dynamic_reconfigure.cfg b/ros/config/dynamic_reconfigure.cfg new file mode 100755 index 00000000..bb901068 --- /dev/null +++ b/ros/config/dynamic_reconfigure.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python +PACKAGE = "dynamic_tutorials" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100) +gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1) +gen.add("str_param", str_t, 0, "A string parameter", "Hello World") +gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) + +size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"), + gen.const("Medium", int_t, 1, "A medium constant"), + gen.const("Large", int_t, 2, "A large constant"), + gen.const("ExtraLarge", int_t, 3, "An extra large constant")], + "An enum to set size") + +gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum) + +exit(gen.generate(PACKAGE, "dynamic_tutorials", "Tutorials")) \ No newline at end of file From e9f8de2d81dedc9242c55a8665540de398dbd09e Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sun, 10 Feb 2019 13:29:54 +0100 Subject: [PATCH 05/88] Fix the Thirdparty folder location --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 34c338b5..54508ce9 100755 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ sudo apt install libeigen3-dev ``` **Required at least 3.1.0**. -## DBoW2 and g2o (Included in Thirdparty folder) +## DBoW2 and g2o (Included in orb_slam2/Thirdparty) 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 From 467c18868c891090411feea32508c20be3d50fa6 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 12 Feb 2019 15:41:10 +0100 Subject: [PATCH 06/88] Adjusted the CMakeList for dynamic_reconfigure; Updated the dynamic_reconfigure .cfg file with the first params from the orb_slam --- CMakeLists.txt | 19 +++++++++++++------ ros/config/dynamic_reconfigure.cfg | 19 +++++-------------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c05cc3b..c11c3eed 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,8 +5,6 @@ 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) @@ -37,12 +35,17 @@ endif() find_package(Eigen3 3.1.0 REQUIRED) -execute_process(COMMAND chmod a+x ros/config/dynamic_reconfigure.cfg +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} @@ -103,7 +106,7 @@ 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} @@ -113,7 +116,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} @@ -123,7 +126,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} @@ -145,6 +148,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/ros/config/dynamic_reconfigure.cfg b/ros/config/dynamic_reconfigure.cfg index bb901068..1c991002 100755 --- a/ros/config/dynamic_reconfigure.cfg +++ b/ros/config/dynamic_reconfigure.cfg @@ -1,21 +1,12 @@ #!/usr/bin/env python -PACKAGE = "dynamic_tutorials" +PACKAGE = "orb_slam2_ros" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100) -gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1) -gen.add("str_param", str_t, 0, "A string parameter", "Hello World") -gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) +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) -size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"), - gen.const("Medium", int_t, 1, "A medium constant"), - gen.const("Large", int_t, 2, "A large constant"), - gen.const("ExtraLarge", int_t, 3, "An extra large constant")], - "An enum to set size") - -gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum) - -exit(gen.generate(PACKAGE, "dynamic_tutorials", "Tutorials")) \ No newline at end of file +exit(gen.generate(PACKAGE, "orb_slam2_ros", "dynamic_reconfigure")) \ No newline at end of file From ab2b5d9cdecce61601c08b07211763c7e020623e Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 12 Feb 2019 15:41:51 +0100 Subject: [PATCH 07/88] Moved dynamic parameters to dynamic_reconfigure --- ros/launch/orb_slam2_mono.launch | 3 --- ros/launch/orb_slam2_rgbd.launch | 5 +---- ros/launch/orb_slam2_stereo.launch | 3 --- 3 files changed, 1 insertion(+), 10 deletions(-) diff --git a/ros/launch/orb_slam2_mono.launch b/ros/launch/orb_slam2_mono.launch index 2446c3ad..8edbd7c6 100644 --- a/ros/launch/orb_slam2_mono.launch +++ b/ros/launch/orb_slam2_mono.launch @@ -7,10 +7,7 @@ - - - diff --git a/ros/launch/orb_slam2_rgbd.launch b/ros/launch/orb_slam2_rgbd.launch index 19109c4b..f264a82a 100644 --- a/ros/launch/orb_slam2_rgbd.launch +++ b/ros/launch/orb_slam2_rgbd.launch @@ -8,10 +8,7 @@ - - - - + > diff --git a/ros/launch/orb_slam2_stereo.launch b/ros/launch/orb_slam2_stereo.launch index 9e2649d1..6a678acb 100644 --- a/ros/launch/orb_slam2_stereo.launch +++ b/ros/launch/orb_slam2_stereo.launch @@ -8,10 +8,7 @@ - - - From 1b700efdb41fb15ea0f67db4ee7985dda1c053a1 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 12 Feb 2019 15:42:21 +0100 Subject: [PATCH 08/88] Implement dynamic_reconfigure --- ros/include/Node.h | 11 ++++++----- ros/src/Node.cc | 35 ++++++++++++++--------------------- 2 files changed, 20 insertions(+), 26 deletions(-) diff --git a/ros/include/Node.h b/ros/include/Node.h index 238e64c6..40ae3e68 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -29,6 +29,9 @@ #include #include +#include +#include + #include #include #include @@ -55,10 +58,11 @@ class Node void PublishMapPoints (std::vector map_points); void PublishPositionAsTransform (cv::Mat position); void PublishRenderedImage (cv::Mat image); - void UpdateParameters (); + void ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level); tf::Transform TransformFromMat (cv::Mat position_mat); 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_; @@ -66,11 +70,8 @@ class Node std::string name_of_node_; ros::NodeHandle node_handle_; - 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_; bool publish_pointcloud_param_; }; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index a136c425..11edfee2 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -7,7 +7,15 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp orb_slam_ = pSLAM; node_handle_ = node_handle; - InitParameters (); + //static parameters + node_handle_.param(name_of_node_+"/publish_pointcloud", publish_pointcloud_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"); + + //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); rendered_image_publisher_ = image_transport.advertise (name_of_node_+"/debug_image", 1); if (publish_pointcloud_param_) { @@ -34,7 +42,6 @@ void Node::Update () { PublishMapPoints (orb_slam_->GetAllMapPoints()); } - UpdateParameters (); } @@ -149,27 +156,13 @@ 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::UpdateParameters () { - node_handle_.param(name_of_node_+"/localize_only", localize_only_param_, false); - orb_slam_->EnableLocalizationOnly (localize_only_param_); +void Node::ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level) { + orb_slam_->EnableLocalizationOnly (config.localize_only); - node_handle_.param(name_of_node_+"/reset_map", reset_map_param_, false); - if (reset_map_param_) { + if (config.reset_map) { orb_slam_->Reset(); - node_handle_.setParam (name_of_node_+"/reset_map", false); + config.reset_map = false; } - 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_); + orb_slam_->SetMinimumKeyFrames (config.min_num_kf_in_map); } From 1670c7c78ea7798b37e9cd1fa2220d9e5f8d54be Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 12 Feb 2019 15:44:25 +0100 Subject: [PATCH 09/88] Noted the possibility to use rqt_reconfigure and the three types of parameters --- README.md | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 34c338b5..ebeb6d07 100755 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon ## Features - Full ROS compatibility - 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 ### Related Publications: @@ -90,15 +90,22 @@ To run the algorithm expects both a vocabulary file (see the paper) and a **conf ## ROS parameters and topics ### 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 from the config file. +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: - **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. - **pointcloud_frame_id**: String. The Frame id of the Pointcloud/map. - **camera_frame_id**: String. The Frame id of the camera position. + +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. +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. @@ -125,3 +132,5 @@ roslaunch orb_slam2_ros orb_slam2_mono.launch roslaunch orb_slam2_ros orb_slam2_stereo.launch roslaunch orb_slam2_ros orb_slam2_rgbd.launch ``` + + From bc67684bb5e63072d8f5a55d87c1663e7d9cb315 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 6 Feb 2019 17:30:56 +0100 Subject: [PATCH 10/88] Add config and launch files for the d435 camera; rename the files for the r200; adjust the readme --- README.md | 20 +++++-- orb_slam2/config/RealSenseD435Mono.yaml | 43 +++++++++++++++ orb_slam2/config/RealSenseD435RGBD.yaml | 52 +++++++++++++++++++ ...lSenseMono.yaml => RealSenseR200Mono.yaml} | 0 ...lSenseRGBD.yaml => RealSenseR200RGBD.yaml} | 0 ...seStereo.yaml => RealSenseR200Stereo.yaml} | 0 ros/launch/orb_slam2_d435_mono.launch | 16 ++++++ ros/launch/orb_slam2_d435_rgbd.launch | 17 ++++++ ...mono.launch => orb_slam2_r200_mono.launch} | 4 +- ...rgbd.launch => orb_slam2_r200_rgbd.launch} | 4 +- ...eo.launch => orb_slam2_r200_stereo.launch} | 4 +- 11 files changed, 151 insertions(+), 9 deletions(-) create mode 100644 orb_slam2/config/RealSenseD435Mono.yaml create mode 100644 orb_slam2/config/RealSenseD435RGBD.yaml rename orb_slam2/config/{RealSenseMono.yaml => RealSenseR200Mono.yaml} (100%) rename orb_slam2/config/{RealSenseRGBD.yaml => RealSenseR200RGBD.yaml} (100%) rename orb_slam2/config/{RealSenseStereo.yaml => RealSenseR200Stereo.yaml} (100%) create mode 100644 ros/launch/orb_slam2_d435_mono.launch create mode 100644 ros/launch/orb_slam2_d435_rgbd.launch rename ros/launch/{orb_slam2_mono.launch => orb_slam2_r200_mono.launch} (77%) rename ros/launch/{orb_slam2_rgbd.launch => orb_slam2_r200_rgbd.launch} (81%) rename ros/launch/{orb_slam2_stereo.launch => orb_slam2_r200_stereo.launch} (80%) diff --git a/README.md b/README.md index 84f89168..5a2b9c12 100755 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon - Data I/O via ROS topics - Parameters can be set with the rqt_reconfigure gui during runtime - Very quick startup through considerably sped up vocab file loading +- Supports the Intel RealSense r200 and D435 out of the box ### 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)**. @@ -128,9 +129,22 @@ source devel/setup.bash ``` you can run the the corresponding nodes with one of the following commands: ``` -roslaunch orb_slam2_ros orb_slam2_mono.launch -roslaunch orb_slam2_ros orb_slam2_stereo.launch -roslaunch orb_slam2_ros orb_slam2_rgbd.launch +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 +roslaunch orb_slam2_ros orb_slam2_d435_mono.launch +roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch +``` +# 5. FAQ +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): +``` +sudo apt install ros-melodic-rgbd-launch ``` diff --git a/orb_slam2/config/RealSenseD435Mono.yaml b/orb_slam2/config/RealSenseD435Mono.yaml new file mode 100644 index 00000000..fcbc28d4 --- /dev/null +++ b/orb_slam2/config/RealSenseD435Mono.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera calibration and distortion parameters (OpenCV) + +Camera.fx: 616.947204 +Camera.fy: 615.890178 +Camera.cx: 327.236045 +Camera.cy: 249.682137 + +Camera.k1: 0.068724 +Camera.k2: -0.067521 +Camera.p1: 0.004235 +Camera.p2: -0.004098 +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/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml new file mode 100644 index 00000000..c83b1df3 --- /dev/null +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -0,0 +1,52 @@ +%YAML:1.0 + +# Camera calibration parameters (OpenCV) +Camera.fx: 616.947204 +Camera.fy: 615.890178 +Camera.cx: 327.236045 +Camera.cy: 249.682137 + +# Camera distortion parameters (OpenCV) +Camera.k1: 0.068724 +Camera.k2: -0.067521 +Camera.p1: 0.004235 +Camera.p2: -0.004098 +Camera.k3: 0.0 + +Camera.width: 640 +Camera.height: 480 + +# IR projector baseline times fx (aprox.) +Camera.bf: 30,8473602 + +# 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/RealSenseMono.yaml b/orb_slam2/config/RealSenseR200Mono.yaml similarity index 100% rename from orb_slam2/config/RealSenseMono.yaml rename to orb_slam2/config/RealSenseR200Mono.yaml diff --git a/orb_slam2/config/RealSenseRGBD.yaml b/orb_slam2/config/RealSenseR200RGBD.yaml similarity index 100% rename from orb_slam2/config/RealSenseRGBD.yaml rename to orb_slam2/config/RealSenseR200RGBD.yaml diff --git a/orb_slam2/config/RealSenseStereo.yaml b/orb_slam2/config/RealSenseR200Stereo.yaml similarity index 100% rename from orb_slam2/config/RealSenseStereo.yaml rename to orb_slam2/config/RealSenseR200Stereo.yaml diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch new file mode 100644 index 00000000..6860168d --- /dev/null +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch new file mode 100644 index 00000000..5042a06f --- /dev/null +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_mono.launch b/ros/launch/orb_slam2_r200_mono.launch similarity index 77% rename from ros/launch/orb_slam2_mono.launch rename to ros/launch/orb_slam2_r200_mono.launch index 8edbd7c6..c8fbb630 100644 --- a/ros/launch/orb_slam2_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -1,8 +1,8 @@ - diff --git a/ros/launch/orb_slam2_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch similarity index 81% rename from ros/launch/orb_slam2_rgbd.launch rename to ros/launch/orb_slam2_r200_rgbd.launch index f264a82a..1481d05e 100644 --- a/ros/launch/orb_slam2_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -1,8 +1,8 @@ - diff --git a/ros/launch/orb_slam2_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch similarity index 80% rename from ros/launch/orb_slam2_stereo.launch rename to ros/launch/orb_slam2_r200_stereo.launch index 6a678acb..7687297c 100644 --- a/ros/launch/orb_slam2_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -1,8 +1,8 @@ - From 0155e5e0852cb9731f2f1c1debb7ebc7a3fbb7d9 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 7 Feb 2019 10:39:56 +0100 Subject: [PATCH 11/88] New calibration parameters for the D435 --- orb_slam2/config/RealSenseD435Mono.yaml | 22 +++++++++++----------- orb_slam2/config/RealSenseD435RGBD.yaml | 20 ++++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/orb_slam2/config/RealSenseD435Mono.yaml b/orb_slam2/config/RealSenseD435Mono.yaml index fcbc28d4..ef3ebdf7 100644 --- a/orb_slam2/config/RealSenseD435Mono.yaml +++ b/orb_slam2/config/RealSenseD435Mono.yaml @@ -1,23 +1,23 @@ %YAML:1.0 # Camera calibration and distortion parameters (OpenCV) - -Camera.fx: 616.947204 -Camera.fy: 615.890178 -Camera.cx: 327.236045 -Camera.cy: 249.682137 - -Camera.k1: 0.068724 -Camera.k2: -0.067521 -Camera.p1: 0.004235 -Camera.p2: -0.004098 +Camera.fx: 613.273638 +Camera.fy: 611.897782 +Camera.cx: 321.819255 +Camera.cy: 251.802312 + +# Camera distortion parameters (OpenCV) +Camera.k1: 0.081516 +Camera.k2: -0.125375 +Camera.p1: 0.005576 +Camera.p2: -0.006738 Camera.k3: 0.0 Camera.width: 640 Camera.height: 480 # Camera frames per second -Camera.fps: 60.0 +Camera.fps: 30.0 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1 diff --git a/orb_slam2/config/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml index c83b1df3..c693526e 100644 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -1,23 +1,23 @@ %YAML:1.0 -# Camera calibration parameters (OpenCV) -Camera.fx: 616.947204 -Camera.fy: 615.890178 -Camera.cx: 327.236045 -Camera.cy: 249.682137 +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 613.273638 +Camera.fy: 611.897782 +Camera.cx: 321.819255 +Camera.cy: 251.802312 # Camera distortion parameters (OpenCV) -Camera.k1: 0.068724 -Camera.k2: -0.067521 -Camera.p1: 0.004235 -Camera.p2: -0.004098 +Camera.k1: 0.081516 +Camera.k2: -0.125375 +Camera.p1: 0.005576 +Camera.p2: -0.006738 Camera.k3: 0.0 Camera.width: 640 Camera.height: 480 # IR projector baseline times fx (aprox.) -Camera.bf: 30,8473602 +Camera.bf: 49.833290 # Camera frames per second Camera.fps: 30.0 From 8bcc43e82c9a30b8b2babc9b1f4c2ac02eb5a73b Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 7 Feb 2019 10:40:34 +0100 Subject: [PATCH 12/88] Now the new config files for the D435 are actually being used --- ros/launch/orb_slam2_d435_mono.launch | 2 +- ros/launch/orb_slam2_d435_rgbd.launch | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 6860168d..fad5d687 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -2,7 +2,7 @@ diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index 5042a06f..59ef3dac 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -2,7 +2,7 @@ From 1c739ca1f35405e8c64a2dbf50a1a307479322a3 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 9 Feb 2019 18:21:55 +0100 Subject: [PATCH 13/88] New parameters for the D435 again, this time from the camera itself --- orb_slam2/config/RealSenseD435Mono.yaml | 16 ++++++++-------- orb_slam2/config/RealSenseD435RGBD.yaml | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/orb_slam2/config/RealSenseD435Mono.yaml b/orb_slam2/config/RealSenseD435Mono.yaml index ef3ebdf7..86d94db7 100644 --- a/orb_slam2/config/RealSenseD435Mono.yaml +++ b/orb_slam2/config/RealSenseD435Mono.yaml @@ -1,16 +1,16 @@ %YAML:1.0 # Camera calibration and distortion parameters (OpenCV) -Camera.fx: 613.273638 -Camera.fy: 611.897782 -Camera.cx: 321.819255 -Camera.cy: 251.802312 +Camera.fx: 615.6707153320312 +Camera.fy: 615.962158203125 +Camera.cx: 328.0010681152344 +Camera.cy: 241.31031799316406 # Camera distortion parameters (OpenCV) -Camera.k1: 0.081516 -Camera.k2: -0.125375 -Camera.p1: 0.005576 -Camera.p2: -0.006738 +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 Camera.k3: 0.0 Camera.width: 640 diff --git a/orb_slam2/config/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml index c693526e..9f095bce 100644 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -1,16 +1,16 @@ %YAML:1.0 # Camera calibration and distortion parameters (OpenCV) -Camera.fx: 613.273638 -Camera.fy: 611.897782 -Camera.cx: 321.819255 -Camera.cy: 251.802312 +Camera.fx: 615.6707153320312 +Camera.fy: 615.962158203125 +Camera.cx: 328.0010681152344 +Camera.cy: 241.31031799316406 # Camera distortion parameters (OpenCV) -Camera.k1: 0.081516 -Camera.k2: -0.125375 -Camera.p1: 0.005576 -Camera.p2: -0.006738 +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 Camera.k3: 0.0 Camera.width: 640 From 4c5c56a0f37038e61b3e0773da39c0efd1e10f7a Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 13 Feb 2019 18:04:44 +0100 Subject: [PATCH 14/88] Adjusted the depth map threshold for the d435_rgbd --- orb_slam2/config/RealSenseD435RGBD.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/orb_slam2/config/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml index 9f095bce..83e4e492 100644 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -28,8 +28,8 @@ Camera.RGB: 1 # Close/Far threshold. Baseline times. ThDepth: 10.0 -# Deptmap values factor -DepthMapFactor: 1.0 +# Deptmap values factor (what pixel value in the depth image corresponds to 1m?) +DepthMapFactor: 1000.0 #-------------------------------------------------------------------------------------------- # ORB Parameters From 5037117383a1fbe67cb450d2108273097f3afd49 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 13 Feb 2019 18:13:27 +0100 Subject: [PATCH 15/88] Fixed the README file --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5a2b9c12..e3fbd4bc 100755 --- a/README.md +++ b/README.md @@ -6,10 +6,10 @@ The original implementation can be found [here](https://github.com/raulmur/ORB_S 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). ## Features - Full ROS compatibility +- Supports the Intel RealSense R200 and D435 out of the box - Data I/O via ROS topics - Parameters can be set with the rqt_reconfigure gui during runtime - Very quick startup through considerably sped up vocab file loading -- Supports the Intel RealSense r200 and D435 out of the box ### 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)**. From 963e964942b46b933274157698a1cf54a0825050 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 15 Feb 2019 15:13:41 +0100 Subject: [PATCH 16/88] Due to issue #5 add the c++11 compiler option again This reverts commit 3c6c96e480f161ed3df85464ae45ad6d120739d2. --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c11c3eed..e8cfd484 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,9 @@ ENDIF() # Check for c++11 support INCLUDE(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -IF(!COMPILER_SUPPORTS_CXX11) +IF(COMPILER_SUPPORTS_CXX11) + add_compile_options(-std=c++11) +ELSE() MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") ENDIF() From 399e75b84fd8b78a94a01ddeb505b98bfa31d766 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 15 Feb 2019 15:29:34 +0100 Subject: [PATCH 17/88] Only compile C++ files with the C++11 flag not C files - fixes the warning --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8cfd484..086ed445 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ ENDIF() INCLUDE(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) IF(COMPILER_SUPPORTS_CXX11) - add_compile_options(-std=c++11) + set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") ELSE() MESSAGE(ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") ENDIF() From 0903bfa20db3b5c0df48d0c087e2f8b90d20bfdf Mon Sep 17 00:00:00 2001 From: Saoto Tsuchiya Date: Sun, 17 Mar 2019 18:16:41 +0900 Subject: [PATCH 18/88] modify transformation matrix of Node::TransformFromMat() --- ros/src/Node.cc | 37 +++++++++++++++---------------------- 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 11edfee2..27c3689d 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -71,8 +71,8 @@ tf::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), rotation.at (1,0), rotation.at (1,1), rotation.at (1,2), @@ -81,29 +81,22 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { tf::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 : same as invYZ * Rz * Rx + const tf::Matrix3x3 matTrans (0, 0, 1, + -1, 0, 0, + 0,-1, 0); - const tf::Matrix3x3 Rz (0, -1, 0, - 1, 0, 0, - 0, 0, 1); + //Convert from orb coordinates to ros coordinates + tf_camera_rotation = matTrans*tf_camera_rotation; + tf_camera_translation = matTrans*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); - - 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; + //Convert from orb coordinates to ros coordinates + tf_camera_rotation = matTrans*tf_camera_rotation; + tf_camera_translation = matTrans*tf_camera_translation; return tf::Transform (tf_camera_rotation, tf_camera_translation); } From d33a0109097f761ee6acf1459c1f721b8ab7eca6 Mon Sep 17 00:00:00 2001 From: Saoto Tsuchiya Date: Sun, 17 Mar 2019 18:30:18 +0900 Subject: [PATCH 19/88] modify transformation matrix of Node::TransformFromMat() --- ros/src/Node.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 27c3689d..59b1a71c 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -81,12 +81,12 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { tf::Vector3 tf_camera_translation (translation.at (0), translation.at (1), translation.at (2)); - //Coordinate transformation matrix : same as invYZ * Rz * Rx + //Coordinate transformation matrix from orb coordinate system to ros coordinate system const tf::Matrix3x3 matTrans (0, 0, 1, -1, 0, 0, 0,-1, 0); - //Convert from orb coordinates to ros coordinates + //Transform from orb coordinate system to ros coordinate system on camera coordinates tf_camera_rotation = matTrans*tf_camera_rotation; tf_camera_translation = matTrans*tf_camera_translation; @@ -94,7 +94,7 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { tf_camera_rotation = tf_camera_rotation.transpose(); tf_camera_translation = -(tf_camera_rotation*tf_camera_translation); - //Convert from orb coordinates to ros coordinates + //Transform from orb coordinate system to ros coordinate system on map coordinates tf_camera_rotation = matTrans*tf_camera_rotation; tf_camera_translation = matTrans*tf_camera_translation; From 303de0c9153c10accce752a3a2150684ed97c5a4 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 15 Feb 2019 15:40:55 +0100 Subject: [PATCH 20/88] Remove the camera name from the node name --- ros/launch/orb_slam2_d435_mono.launch | 2 +- ros/launch/orb_slam2_d435_rgbd.launch | 2 +- ros/launch/orb_slam2_r200_mono.launch | 2 +- ros/launch/orb_slam2_r200_rgbd.launch | 2 +- ros/launch/orb_slam2_r200_stereo.launch | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index fad5d687..78269d73 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -1,5 +1,5 @@ - - - - - Date: Mon, 18 Mar 2019 10:43:14 +0100 Subject: [PATCH 21/88] Adjsut the individual depth thresholds for the RGBD cams --- orb_slam2/config/RealSenseD435RGBD.yaml | 2 +- orb_slam2/config/RealSenseR200RGBD.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/orb_slam2/config/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml index 83e4e492..3fe800ac 100644 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -26,7 +26,7 @@ Camera.fps: 30.0 Camera.RGB: 1 # Close/Far threshold. Baseline times. -ThDepth: 10.0 +ThDepth: 40.0 # Deptmap values factor (what pixel value in the depth image corresponds to 1m?) DepthMapFactor: 1000.0 diff --git a/orb_slam2/config/RealSenseR200RGBD.yaml b/orb_slam2/config/RealSenseR200RGBD.yaml index aa1d0e30..6af422c0 100644 --- a/orb_slam2/config/RealSenseR200RGBD.yaml +++ b/orb_slam2/config/RealSenseR200RGBD.yaml @@ -26,7 +26,7 @@ Camera.fps: 30.0 Camera.RGB: 1 # Close/Far threshold. Baseline times. -ThDepth: 10.0 +ThDepth: 20.0 # Deptmap values factor DepthMapFactor: 1.0 From 35b7f05fd5cd176f6a6aa17fe63f8ecbe189c62d Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 18 Mar 2019 10:50:26 +0100 Subject: [PATCH 22/88] Fix variable naming --- ros/src/Node.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 59b1a71c..4b247677 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -82,21 +82,21 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { tf::Vector3 tf_camera_translation (translation.at (0), translation.at (1), translation.at (2)); //Coordinate transformation matrix from orb coordinate system to ros coordinate system - const tf::Matrix3x3 matTrans (0, 0, 1, - -1, 0, 0, - 0,-1, 0); + const tf::Matrix3x3 tf_orb_to_ros (0, 0, 1, + -1, 0, 0, + 0,-1, 0); //Transform from orb coordinate system to ros coordinate system on camera coordinates - tf_camera_rotation = matTrans*tf_camera_rotation; - tf_camera_translation = matTrans*tf_camera_translation; + tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation; + tf_camera_translation = tf_orb_to_ros*tf_camera_translation; //Inverse matrix tf_camera_rotation = tf_camera_rotation.transpose(); tf_camera_translation = -(tf_camera_rotation*tf_camera_translation); //Transform from orb coordinate system to ros coordinate system on map coordinates - tf_camera_rotation = matTrans*tf_camera_rotation; - tf_camera_translation = matTrans*tf_camera_translation; + tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation; + tf_camera_translation = tf_orb_to_ros*tf_camera_translation; return tf::Transform (tf_camera_rotation, tf_camera_translation); } From 4368278d5ebe6b1866460b30243cbb41c2142ed3 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 18 Mar 2019 11:37:40 +0100 Subject: [PATCH 23/88] Add an overview on how to use different cameras --- README.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e3fbd4bc..a1a0f26e 100755 --- a/README.md +++ b/README.md @@ -136,6 +136,14 @@ roslaunch orb_slam2_ros orb_slam2_d435_mono.launch roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch ``` # 5. FAQ +Here are some answers to frequently asked questions. +### Using a new / different camera +You can use this SLAM with almost any mono, stereo or RGBD cam you want. +There are two files which need to be adjusted for a new camera: +1) **The yaml config file** at orb_slam2/config for 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 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 need to adjust the other parameters such as 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. + +### Problem running the realsense node The node for the RealSense fails to launch when running ``` roslaunch realsense2_camera rs_rgbd.launch @@ -146,5 +154,3 @@ install the rgbd-launch package with the command (make sure to adjust the ROS di ``` sudo apt install ros-melodic-rgbd-launch ``` - - From 5ab9c0b7b759bfba12e9430b0b825a18f033ae97 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 18 Mar 2019 22:05:26 +0100 Subject: [PATCH 24/88] Enable the possibility to configurer the minimal number of observations a point must have to get into the ros point cloud --- ros/config/dynamic_reconfigure.cfg | 9 +++++---- ros/include/Node.h | 1 + ros/src/Node.cc | 4 +++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/ros/config/dynamic_reconfigure.cfg b/ros/config/dynamic_reconfigure.cfg index 1c991002..c8365689 100755 --- a/ros/config/dynamic_reconfigure.cfg +++ b/ros/config/dynamic_reconfigure.cfg @@ -5,8 +5,9 @@ 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("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")) \ No newline at end of file +exit(gen.generate(PACKAGE, "orb_slam2_ros", "dynamic_reconfigure")) diff --git a/ros/include/Node.h b/ros/include/Node.h index 40ae3e68..2bfd1090 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -73,6 +73,7 @@ class Node std::string map_frame_id_param_; std::string camera_frame_id_param_; bool publish_pointcloud_param_; + int min_observations_per_point_; }; #endif //ORBSLAM2_ROS_NODE_H_ diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 4b247677..cdf4a052 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -6,6 +6,7 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp name_of_node_ = ros::this_node::getName(); orb_slam_ = pSLAM; node_handle_ = node_handle; + min_observations_per_point_ = 2; //static parameters node_handle_.param(name_of_node_+"/publish_pointcloud", publish_pointcloud_param_, true); @@ -135,7 +136,7 @@ sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vectorisBad()) { + if (map_points.at(i)->nObs >= min_observations_per_point_) {//nObs isBad() 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 @@ -151,6 +152,7 @@ sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vectorEnableLocalizationOnly (config.localize_only); + min_observations_per_point_ = config.min_observations_for_ros_map; if (config.reset_map) { orb_slam_->Reset(); From 9aed4bf4ea1177f8e2ddc2a47e12aed40890559a Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 18 Mar 2019 22:10:33 +0100 Subject: [PATCH 25/88] Add explanation for the new min_observation param --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index a1a0f26e..b28e358e 100755 --- a/README.md +++ b/README.md @@ -104,6 +104,7 @@ 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. From 664c6cd59122debedc24827012d59ba1bab7f6e0 Mon Sep 17 00:00:00 2001 From: hoangthien94 Date: Mon, 8 Apr 2019 14:37:23 +0800 Subject: [PATCH 26/88] Add support for MyntEye S camera --- orb_slam2/config/mynteye_s_mono.yaml | 63 ++++++++++ orb_slam2/config/mynteye_s_stereo.yaml | 116 +++++++++++++++++++ ros/launch/orb_slam2_mynteye_s_mono.launch | 16 +++ ros/launch/orb_slam2_mynteye_s_stereo.launch | 14 +++ 4 files changed, 209 insertions(+) create mode 100644 orb_slam2/config/mynteye_s_mono.yaml create mode 100644 orb_slam2/config/mynteye_s_stereo.yaml create mode 100644 ros/launch/orb_slam2_mynteye_s_mono.launch create mode 100644 ros/launch/orb_slam2_mynteye_s_stereo.launch diff --git a/orb_slam2/config/mynteye_s_mono.yaml b/orb_slam2/config/mynteye_s_mono.yaml new file mode 100644 index 00000000..52c521e1 --- /dev/null +++ b/orb_slam2/config/mynteye_s_mono.yaml @@ -0,0 +1,63 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 332.97713134460906 +Camera.fy: 332.97713134460906 +Camera.cx: 398.9270935058594 +Camera.cy: 252.28187370300293 + +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30.0 + +# stereo baseline times fx +Camera.bf: 47.90639384423901 + +# 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: 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 + +#-------------------------------------------------------------------------------------------- +# 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/mynteye_s_stereo.yaml b/orb_slam2/config/mynteye_s_stereo.yaml new file mode 100644 index 00000000..032b6ee9 --- /dev/null +++ b/orb_slam2/config/mynteye_s_stereo.yaml @@ -0,0 +1,116 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 332.97713134460906 +Camera.fy: 332.97713134460906 +Camera.cx: 398.9270935058594 +Camera.cy: 252.28187370300293 + +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30.0 + +# stereo baseline times fx +Camera.bf: 47.90639384423901 + +# 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: 35 + +#-------------------------------------------------------------------------------------------- +# 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: 480 +LEFT.width: 752 +LEFT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [-0.27186431380704884, 0.05397709169334604, -0.000557307377524114, -0.0006127379205397152, 0.0] +LEFT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [360.0652897865692, 0.0, 406.6650580307593, 0.0, 363.2195731683743, 256.20533579714373, 0.0, 0.0, 1.0] +LEFT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9995051975828172, -0.004232452030025982, 0.031168034181628675, 0.00404399044045864, 0.9999731738419173, 0.006107187391924209, -0.031193046440691295, -0.005978122308562103, 0.9994955006939314] +LEFT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [332.97713134460906, 0.0, 398.9270935058594, 0.0, 0.0, 332.97713134460906, 252.28187370300293, 0.0, 0.0, 0.0, 1.0, 0.0] + +RIGHT.height: 480 +RIGHT.width: 752 +RIGHT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data:[-0.2608724975841786, 0.04949670859088579, -0.00033079602477046385, 0.002888671484465195, 0.0] +RIGHT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [362.3216398160725, 0.0, 356.12189598153714, 0.0, 365.90199497184676, 255.37341022147191, 0.0, 0.0, 1.0] +RIGHT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9971385096122076, -0.004423319050556735, -0.07546672708500173, 0.003966835741299504, 0.9999729264511134, -0.006197626884355805, 0.0754920980139425, 0.005880528324319419, 0.9971290601141259] +RIGHT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [332.97713134460906, 0.0, 398.9270935058594, -39.71778017327359, 0.0, 332.97713134460906, 252.28187370300293, 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 + +#-------------------------------------------------------------------------------------------- +# 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/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch new file mode 100644 index 00000000..a5ba572c --- /dev/null +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + 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..8cd81b56 --- /dev/null +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -0,0 +1,14 @@ + + + + + + + + + + From c070d1055225721f1349efdc1823bd23b3547137 Mon Sep 17 00:00:00 2001 From: hoangthien94 Date: Tue, 9 Apr 2019 13:56:38 +0800 Subject: [PATCH 27/88] Add PoseStamped publisher --- ros/include/Node.h | 4 ++++ ros/src/Node.cc | 14 ++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/ros/include/Node.h b/ros/include/Node.h index 2bfd1090..42369908 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -37,6 +37,7 @@ #include #include #include +#include #include "System.h" @@ -57,6 +58,7 @@ class Node private: void PublishMapPoints (std::vector map_points); void PublishPositionAsTransform (cv::Mat position); + void PublishPositionAsPoseStamped(cv::Mat position); void PublishRenderedImage (cv::Mat image); void ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level); tf::Transform TransformFromMat (cv::Mat position_mat); @@ -66,6 +68,7 @@ class Node image_transport::Publisher rendered_image_publisher_; ros::Publisher map_points_publisher_; + ros::Publisher pose_publisher_; std::string name_of_node_; ros::NodeHandle node_handle_; @@ -73,6 +76,7 @@ class Node std::string map_frame_id_param_; std::string camera_frame_id_param_; bool publish_pointcloud_param_; + bool publish_pose_param_; int min_observations_per_point_; }; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index cdf4a052..299d4372 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -10,6 +10,7 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp //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_+"/pointcloud_frame_id", map_frame_id_param_, "map"); node_handle_.param(name_of_node_+"/camera_frame_id", camera_frame_id_param_, "camera_link"); @@ -22,6 +23,7 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp if (publish_pointcloud_param_) { map_points_publisher_ = node_handle_.advertise (name_of_node_+"/map_points", 1); } + pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); } @@ -35,6 +37,10 @@ void Node::Update () { if (!position.empty()) { PublishPositionAsTransform (position); + + if (publish_pose_param_) { + PublishPositionAsPoseStamped (position); + } } PublishRenderedImage (orb_slam_->DrawCurrentFrame()); @@ -58,6 +64,14 @@ void Node::PublishPositionAsTransform (cv::Mat position) { tf_broadcaster.sendTransform(tf::StampedTransform(transform, current_frame_time_, map_frame_id_param_, camera_frame_id_param_)); } +void Node::PublishPositionAsPoseStamped (cv::Mat position) { + tf::Transform grasp_tf = TransformFromMat (position); + tf::Stamped grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_); + geometry_msgs::PoseStamped pose_msg; + tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg); + pose_publisher_.publish(pose_msg); +} + void Node::PublishRenderedImage (cv::Mat image) { std_msgs::Header header; From 007c4dbd8deaabceb14d5b0e0d7f1c4fcc593c32 Mon Sep 17 00:00:00 2001 From: hoangthien94 Date: Tue, 9 Apr 2019 14:05:49 +0800 Subject: [PATCH 28/88] Add param to enable/disable publishing pose --- ros/launch/orb_slam2_d435_mono.launch | 1 + ros/launch/orb_slam2_d435_rgbd.launch | 1 + ros/launch/orb_slam2_mynteye_s_mono.launch | 1 + ros/launch/orb_slam2_mynteye_s_stereo.launch | 1 + ros/launch/orb_slam2_r200_mono.launch | 1 + ros/launch/orb_slam2_r200_rgbd.launch | 1 + ros/launch/orb_slam2_r200_stereo.launch | 1 + ros/src/Node.cc | 6 +++++- 8 files changed, 12 insertions(+), 1 deletion(-) diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 78269d73..9fb19934 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -7,6 +7,7 @@ + diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index ca32f08d..1689e528 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -8,6 +8,7 @@ + diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch index a5ba572c..cc03b525 100644 --- a/ros/launch/orb_slam2_mynteye_s_mono.launch +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -7,6 +7,7 @@ + diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch index 8cd81b56..790eec02 100644 --- a/ros/launch/orb_slam2_mynteye_s_stereo.launch +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -8,6 +8,7 @@ + diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch index 2791a942..50846c32 100644 --- a/ros/launch/orb_slam2_r200_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -7,6 +7,7 @@ + diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch index 86b43b11..f5b80c6e 100644 --- a/ros/launch/orb_slam2_r200_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -8,6 +8,7 @@ + > diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch index a4b1d5e6..48b52364 100644 --- a/ros/launch/orb_slam2_r200_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -8,6 +8,7 @@ + diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 299d4372..2c796669 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -23,7 +23,11 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp if (publish_pointcloud_param_) { map_points_publisher_ = node_handle_.advertise (name_of_node_+"/map_points", 1); } - pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); + + // Enable publishing camera's pose as PoseStamped message + if (publish_pose_param_) { + pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); + } } From d17b87fff686ad3b7ca03591bc60268ae5690007 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 9 Apr 2019 14:37:54 +0200 Subject: [PATCH 29/88] Add the cmake-build-debug folder to the .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index b7c2a180..8c9b43cd 100755 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ CMakeLists.txt.user .idea/ +cmake-build-debug KeyFrameTrajectory.txt Thirdparty/DBoW2/build/ Thirdparty/DBoW2/lib/ From 9b8661e692f0911285b4c9131e4ad62c6b68c120 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 9 Apr 2019 14:39:33 +0200 Subject: [PATCH 30/88] Make the installation of dependencies more clear; Add a note for the publish_pose param; Add a table containing all cameras which are supported out of the box --- README.md | 62 ++++++++++++++++++++++++++++--------------------------- 1 file changed, 32 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index b28e358e..9826c211 100755 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ The original implementation can be found [here](https://github.com/raulmur/ORB_S 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). ## Features - Full ROS compatibility -- Supports the Intel RealSense R200 and D435 out of the box +- 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 with the rqt_reconfigure gui during runtime - Very quick startup through considerably sped up vocab file loading @@ -49,35 +49,34 @@ 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 the new thread and chrono functionalities of C++11. +## Getting the code +Clone the repository into your catkin workspace: +``` +git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git +``` -## OpenCV -We use [OpenCV](http://opencv.org) to manipulate images and features. OpenCV should be installed along with ROS. +## 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**. - -## DBoW2 and g2o (Included in orb_slam2/Thirdparty) -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. +**Required at least Eigen 3.1.0**. -# 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 ``` @@ -85,7 +84,7 @@ catkin build ``` in your catkin folder. -# 4. Configuration +# 3. 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. @@ -95,6 +94,7 @@ There are three types of parameters right now: static- and dynamic ros parameter 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: - **publish_pointcloud**: Bool. If the pointcloud containing all key points (the map) should be published. +- **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. @@ -111,6 +111,7 @@ Finally, the intrinsic camera calibration parameters along with some hyperparame ### 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. - 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). @@ -123,19 +124,20 @@ The following topics are being published and subscribed to by the nodes: - The stereo node subscribes to **image_left/image_color_rect** and - **image_right/image_color_rect** for corresponding images. -# 5. Run +# 4. Run After sourcing your setup bash using ``` source devel/setup.bash ``` -you can run the the corresponding nodes with one of the following commands: -``` -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 -roslaunch orb_slam2_ros orb_slam2_d435_mono.launch -roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch -``` +## 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 ``` | - | | | | | + +Use the command from the corresponding cell for your camera to launch orb_slam2_ros with the right parameters for your setup. + # 5. FAQ Here are some answers to frequently asked questions. ### Using a new / different camera From 6da6e280c9171209daf287326570d6f41a9086ba Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 19 Mar 2019 13:24:29 +0100 Subject: [PATCH 31/88] Use the num_channels for the payload too --- ros/src/Node.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 2c796669..c53822e2 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -152,15 +152,15 @@ sensor_msgs::PointCloud2 Node::MapPointsToPointCloud (std::vectornObs >= min_observations_per_point_) {//nObs isBad() + 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)); } } From a4211fb9be9a2582454d9709c3e18045419ce6c9 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 17:18:04 +0200 Subject: [PATCH 32/88] Give the StereoNode class owenership of the subscribers and sync --- ros/include/StereoNode.h | 10 ++++++++++ ros/src/StereoNode.cc | 16 ++++++++-------- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ros/include/StereoNode.h b/ros/include/StereoNode.h index fed7c7cf..4e656eb5 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -6,6 +6,9 @@ #include #include +#include +#include +#include #include #include #include @@ -22,5 +25,12 @@ class StereoNode : public Node ~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_; + + }; diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index ee8bb0d4..711f847c 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -21,17 +21,9 @@ int main(int argc, char **argv) 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); - // register callbacks - sync.registerCallback(boost::bind(&StereoNode::ImageCallback, &node,_1,_2)); - ros::spin(); // Stop all threads @@ -42,10 +34,18 @@ int main(int argc, char **argv) StereoNode::StereoNode (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) : Node (pSLAM, 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); + + 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_; } From 2c866786e7a42576d4c9b8683b9c50abf3efde79 Mon Sep 17 00:00:00 2001 From: Brahim Boudamouz Date: Mon, 24 Jun 2019 15:26:08 +0200 Subject: [PATCH 33/88] add save and load feature --- CMakeLists.txt | 8 + orb_slam2/include/BoostArchiver.h | 95 ++++++++++++ orb_slam2/include/KeyFrame.h | 12 ++ orb_slam2/include/KeyFrameDatabase.h | 13 ++ orb_slam2/include/Map.h | 10 +- orb_slam2/include/MapPoint.h | 11 ++ orb_slam2/include/System.h | 15 +- orb_slam2/src/KeyFrame.cc | 85 +++++++++++ orb_slam2/src/KeyFrameDatabase.cc | 16 ++ orb_slam2/src/Map.cc | 16 ++ orb_slam2/src/MapPoint.cc | 41 ++++- orb_slam2/src/System.cc | 85 ++++++++++- ros/config/rviz_config.rviz | 197 +++++++++++++++++++++++++ ros/launch/orb_slam2_d435i_rgbd.launch | 29 ++++ ros/src/RGBDNode.cc | 20 ++- 15 files changed, 640 insertions(+), 13 deletions(-) create mode 100644 orb_slam2/include/BoostArchiver.h create mode 100644 ros/config/rviz_config.rviz create mode 100644 ros/launch/orb_slam2_d435i_rgbd.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 086ed445..4de447d3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,14 @@ 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 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/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/System.h b/orb_slam2/include/System.h index 37776f9b..3b3fddae 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -56,7 +56,8 @@ 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 string &strSettingsFile, const eSensor sensor, + const std::string & map_file = "", bool save_map = false, bool load_map = false); // map serialization addition // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. @@ -182,6 +183,18 @@ class System // Current position cv::Mat current_position_; + +// map serialization addition +private: + // Save/Load functions + bool save_map; + bool load_map; + std::string map_file; + bool LoadMap(const string &filename); +public: + void SaveMap(const string &filename); + + }; }// namespace ORB_SLAM 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/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 c0c8690a..f66f7f2e 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 string &strSettingsFile, const eSensor sensor, + const std::string & map_file, bool save_map, bool load_map): // map serialization addition + mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), + map_file(map_file), save_map(save_map), load_map(load_map) { // Output welcome message cout << endl << @@ -83,11 +84,21 @@ 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)) { + currently_localizing_only_ = true; + ActivateLocalizationMode(); + 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(); + currently_localizing_only_ = false; + } + // end map serialization addition //Create Drawers. These are used by the Viewer mpFrameDrawer = new FrameDrawer(mpMap); @@ -510,6 +521,64 @@ void System::EnableLocalizationOnly (bool localize_only) { DeactivateLocalizationMode(); } } + + std::cout << "Enable localization only: " << (localize_only?"true":"false") << std::endl; +} + + +// map serialization addition +void 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; + exit(-1); + } + + 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(); +} + +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; + } + + 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(); + + return true; } } //namespace ORB_SLAM 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/launch/orb_slam2_d435i_rgbd.launch b/ros/launch/orb_slam2_d435i_rgbd.launch new file mode 100644 index 00000000..1480e330 --- /dev/null +++ b/ros/launch/orb_slam2_d435i_rgbd.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc index 796766fb..2a256cfb 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -12,10 +12,22 @@ int main(int argc, char **argv) return 1; } - // 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); + // begin - map serialization addition + // Initialize node handle ros::NodeHandle node_handle; + std::string name_of_node = ros::this_node::getName(); + + // Map serialization parameters + std::string map_file; + bool save_map, load_map; + node_handle.param(name_of_node + "/map_file", map_file, "map.bin"); + node_handle.param(name_of_node + "/save_map", save_map, false); + node_handle.param(name_of_node + "/load_map", load_map, false); + + // 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, map_file, save_map, load_map); image_transport::ImageTransport image_transport (node_handle); + // end - map serialization addition RGBDNode node (&SLAM, node_handle, image_transport); @@ -27,6 +39,10 @@ int main(int argc, char **argv) // Save camera trajectory SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + if(save_map) { + SLAM.SaveMap(map_file); + } + ros::shutdown(); return 0; From 975319a1533398764a7b7ae2f5eaa5aa034d2617 Mon Sep 17 00:00:00 2001 From: Brahim Boudamouz Date: Mon, 24 Jun 2019 16:00:17 +0200 Subject: [PATCH 34/88] correction of launch file --- ros/launch/orb_slam2_d435i_rgbd.launch | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ros/launch/orb_slam2_d435i_rgbd.launch b/ros/launch/orb_slam2_d435i_rgbd.launch index 1480e330..ea689a61 100644 --- a/ros/launch/orb_slam2_d435i_rgbd.launch +++ b/ros/launch/orb_slam2_d435i_rgbd.launch @@ -11,10 +11,11 @@ + - + @@ -22,8 +23,4 @@ - - - - From 0039654ead26394d5373665ef38a336888f231a5 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 8 Jul 2019 11:02:35 +0200 Subject: [PATCH 35/88] Implement the save load feature for all three SLAM types; DDreconfigure crashes after launch --- orb_slam2/include/System.h | 2 +- orb_slam2/src/System.cc | 11 ++++----- ros/include/MonoNode.h | 2 +- ros/include/Node.h | 7 +++++- ros/include/RGBDNode.h | 2 +- ros/include/StereoNode.h | 2 +- ros/launch/orb_slam2_d435_rgbd.launch | 14 +++++++---- ros/launch/orb_slam2_d435i_rgbd.launch | 26 -------------------- ros/src/MonoNode.cc | 18 ++++---------- ros/src/Node.cc | 29 +++++++++++++++++----- ros/src/RGBDNode.cc | 33 ++++---------------------- ros/src/StereoNode.cc | 14 +++-------- 12 files changed, 59 insertions(+), 101 deletions(-) delete mode 100644 ros/launch/orb_slam2_d435i_rgbd.launch diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 3b3fddae..529e0ffe 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -56,7 +56,7 @@ 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 string strSettingsFile, const eSensor sensor, const std::string & map_file = "", bool save_map = false, bool load_map = false); // map serialization addition // Proccess the given stereo frame. Images must be synchronized and rectified. diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index f66f7f2e..a5e2a220 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -28,7 +28,7 @@ namespace ORB_SLAM2 { -System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, +System::System(const string strVocFile, const string strSettingsFile, const eSensor sensor, const std::string & map_file, bool save_map, bool load_map): // map serialization addition mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), map_file(map_file), save_map(save_map), load_map(load_map) @@ -69,13 +69,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; @@ -87,8 +87,6 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS // begin map serialization addition // load serialized map if (load_map && LoadMap(map_file)) { - currently_localizing_only_ = true; - ActivateLocalizationMode(); std::cout << "Using loaded map with " << mpMap->MapPointsInMap() << " points\n" << std::endl; } else { @@ -96,7 +94,6 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); //Create the Map mpMap = new Map(); - currently_localizing_only_ = false; } // end map serialization addition @@ -502,12 +499,14 @@ std::vector System::GetAllMapPoints() { 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; } 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 42369908..0f9eda23 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -46,7 +46,7 @@ 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 (); protected: @@ -75,6 +75,11 @@ class Node std::string map_frame_id_param_; std::string camera_frame_id_param_; + std::string map_file_name_param_; + std::string voc_file_name_param_; + std::string settings_file_name_param_; + bool save_map_param_; + bool load_map_param_; bool publish_pointcloud_param_; bool publish_pose_param_; int min_observations_per_point_; 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 4e656eb5..b8365b45 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -21,7 +21,7 @@ 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); diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index 1689e528..e3caa86c 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -1,9 +1,7 @@ + type="orb_slam2_ros_rgbd" output="screen"> + @@ -11,6 +9,14 @@ + + + + + + + + diff --git a/ros/launch/orb_slam2_d435i_rgbd.launch b/ros/launch/orb_slam2_d435i_rgbd.launch deleted file mode 100644 index ea689a61..00000000 --- a/ros/launch/orb_slam2_d435i_rgbd.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc index d573f0d4..8287feaa 100644 --- a/ros/src/MonoNode.cc +++ b/ros/src/MonoNode.cc @@ -5,35 +5,25 @@ 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); + MonoNode node (ORB_SLAM2::System::MONOCULAR, node_handle, image_transport); ros::spin(); - // Stop all threads - SLAM.Shutdown(); - - // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - ros::shutdown(); return 0; } -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); } diff --git a/ros/src/Node.cc b/ros/src/Node.cc index c53822e2..b804b7f6 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -2,17 +2,21 @@ #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) { name_of_node_ = ros::this_node::getName(); - orb_slam_ = pSLAM; node_handle_ = node_handle; min_observations_per_point_ = 2; //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_+"/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_+ "/publish_pointcloud", publish_pointcloud_param_, true); + node_handle_.param(name_of_node_+ "/publish_pose", publish_pose_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_ + "/map_file", map_file_name_param_, "map.bin"); + node_handle_.param(name_of_node_ + "/voc_file", voc_file_name_param_, "orb_slam2/Vocabulary/ORBvoc.txt"); + node_handle_.param(name_of_node_ + "/settings_file", settings_file_name_param_, "file_not_set"); + node_handle_.param(name_of_node_ + "/save_map", save_map_param_, false); + node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); //Setup dynamic reconfigure dynamic_reconfigure::Server::CallbackType dynamic_param_callback; @@ -28,11 +32,24 @@ Node::Node (ORB_SLAM2::System* pSLAM, ros::NodeHandle &node_handle, image_transp if (publish_pose_param_) { pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); } + + orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, save_map_param_, load_map_param_); + } Node::~Node () { + // Stop all threads + orb_slam_->Shutdown(); + // Save camera trajectory + orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + + // Save map + if(save_map_param_) { + orb_slam_->SaveMap(map_file_name_param_); + } + delete orb_slam_; } diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc index 2a256cfb..c33de708 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -5,51 +5,26 @@ 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."); } - // begin - map serialization addition - // Initialize node handle ros::NodeHandle node_handle; - std::string name_of_node = ros::this_node::getName(); - - // Map serialization parameters - std::string map_file; - bool save_map, load_map; - node_handle.param(name_of_node + "/map_file", map_file, "map.bin"); - node_handle.param(name_of_node + "/save_map", save_map, false); - node_handle.param(name_of_node + "/load_map", load_map, false); // 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, map_file, save_map, load_map); image_transport::ImageTransport image_transport (node_handle); - // end - map serialization addition - RGBDNode node (&SLAM, node_handle, image_transport); + RGBDNode node (ORB_SLAM2::System::RGBD, node_handle, image_transport); ros::spin(); - // Stop all threads - SLAM.Shutdown(); - - // Save camera trajectory - SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - - if(save_map) { - SLAM.SaveMap(map_file); - } - ros::shutdown(); return 0; } -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); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index 711f847c..df409c56 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -9,26 +9,18 @@ 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); // initilaize - StereoNode node (&SLAM, node_handle, image_transport); + StereoNode node (ORB_SLAM2::System::STEREO, node_handle, image_transport); ros::spin(); - // Stop all threads - SLAM.Shutdown(); - return 0; } From 4d9a63be71b933846e81d45afc0a8824b72f0a63 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 16:33:43 +0200 Subject: [PATCH 36/88] Fix the crash; Adjust all the launch file --- ros/launch/orb_slam2_d435_mono.launch | 16 +++++++++++----- ros/launch/orb_slam2_d435_rgbd.launch | 4 ++-- ros/launch/orb_slam2_mynteye_s_mono.launch | 13 +++++++++---- ros/launch/orb_slam2_mynteye_s_stereo.launch | 19 ++++++++++++++----- ros/launch/orb_slam2_r200_mono.launch | 19 ++++++++++++++----- ros/launch/orb_slam2_r200_rgbd.launch | 19 ++++++++++++++----- ros/launch/orb_slam2_r200_stereo.launch | 17 +++++++++++++---- ros/src/Node.cc | 7 ++++--- 8 files changed, 81 insertions(+), 33 deletions(-) diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 9fb19934..4dc43712 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -1,17 +1,23 @@ + type="orb_slam2_ros_mono" output="screen"> + + + + + + + + + - + \ No newline at end of file diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index e3caa86c..1bac88c4 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -14,8 +14,8 @@ - - + + diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch index cc03b525..b1fafe30 100644 --- a/ros/launch/orb_slam2_mynteye_s_mono.launch +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -1,15 +1,20 @@ + type="orb_slam2_ros_mono" output="screen"> + + + + + + + + diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch index 790eec02..20b1d180 100644 --- a/ros/launch/orb_slam2_mynteye_s_stereo.launch +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -1,15 +1,24 @@ + type="orb_slam2_ros_stereo" output="screen"> + + + + + + + + + + + + - + \ No newline at end of file diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch index 50846c32..8ea02c91 100644 --- a/ros/launch/orb_slam2_r200_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -1,14 +1,23 @@ + type="orb_slam2_ros_mono" output="screen"> + + + + + + + + + + + + - + \ No newline at end of file diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch index f5b80c6e..d2bc6704 100644 --- a/ros/launch/orb_slam2_r200_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -1,15 +1,24 @@ + type="orb_slam2_ros_rgbd" output="screen"> + + + + + + + + + + + - > + + diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch index 48b52364..600b4a6c 100644 --- a/ros/launch/orb_slam2_r200_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -1,15 +1,24 @@ + type="orb_slam2_ros_stereo" output="screen"> + + + + + + + + + + + + diff --git a/ros/src/Node.cc b/ros/src/Node.cc index b804b7f6..d3979161 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -13,11 +13,14 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima 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_ + "/map_file", map_file_name_param_, "map.bin"); - node_handle_.param(name_of_node_ + "/voc_file", voc_file_name_param_, "orb_slam2/Vocabulary/ORBvoc.txt"); + node_handle_.param(name_of_node_ + "/voc_file", voc_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/settings_file", settings_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/save_map", save_map_param_, false); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); + + orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, save_map_param_, load_map_param_); + //Setup dynamic reconfigure dynamic_reconfigure::Server::CallbackType dynamic_param_callback; dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2); @@ -33,8 +36,6 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); } - orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, save_map_param_, load_map_param_); - } From cbefb673bae44a426eab08882cf002b7f560b14f Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 18:56:13 +0200 Subject: [PATCH 37/88] Map save is now offered as a service instead as a parameter; Adjusted the launch files; Made the map save more verbose in case of a crash --- CMakeLists.txt | 13 +++++++- orb_slam2/include/System.h | 22 ++++++-------- orb_slam2/src/System.cc | 31 +++++++++++++------- package.xml | 2 ++ ros/include/Node.h | 7 ++++- ros/launch/orb_slam2_d435_mono.launch | 1 - ros/launch/orb_slam2_d435_rgbd.launch | 3 +- ros/launch/orb_slam2_mynteye_s_mono.launch | 1 - ros/launch/orb_slam2_mynteye_s_stereo.launch | 1 - ros/launch/orb_slam2_r200_mono.launch | 1 - ros/launch/orb_slam2_r200_rgbd.launch | 1 - ros/launch/orb_slam2_r200_stereo.launch | 1 - ros/src/Node.cc | 19 ++++++++---- srv/SaveMap.srv | 2 ++ 14 files changed, 66 insertions(+), 39 deletions(-) create mode 100644 srv/SaveMap.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 4de447d3..10412bf0 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ image_transport tf sensor_msgs dynamic_reconfigure +message_generation ) find_package(OpenCV 3.0 QUIET) @@ -61,8 +62,18 @@ ${OpenCV_LIBS} ${catkin_LIBRARIES} ) +add_service_files( + FILES + SaveMap.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + catkin_package ( - CATKIN_DEPENDS roscpp rospy std_msgs cv_bridge image_transport tf sensor_msgs dynamic_reconfigure + CATKIN_DEPENDS roscpp rospy std_msgs cv_bridge image_transport tf sensor_msgs dynamic_reconfigure message_runtime LIBRARIES {PROJECT_NAME} libDBoW2 libg2o ) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 529e0ffe..37148b4f 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -57,7 +57,7 @@ class System // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. System(const string strVocFile, const string strSettingsFile, const eSensor sensor, - const std::string & map_file = "", bool save_map = false, bool load_map = false); // map serialization addition + const std::string & map_file = "", bool load_map = false); // map serialization addition // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. @@ -114,6 +114,8 @@ class System void SetMinimumKeyFrames (int min_num_kf); + bool SaveMap(const string &filename); + cv::Mat GetCurrentPosition (); // Information from most recent processed frame @@ -133,8 +135,14 @@ class System // 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; @@ -183,18 +191,6 @@ class System // Current position cv::Mat current_position_; - -// map serialization addition -private: - // Save/Load functions - bool save_map; - bool load_map; - std::string map_file; - bool LoadMap(const string &filename); -public: - void SaveMap(const string &filename); - - }; }// namespace ORB_SLAM diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index a5e2a220..38f6f580 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -29,9 +29,9 @@ namespace ORB_SLAM2 { System::System(const string strVocFile, const string strSettingsFile, const eSensor sensor, - const std::string & map_file, bool save_map, bool load_map): // map serialization addition + const std::string & map_file, bool load_map): // map serialization addition mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), - map_file(map_file), save_map(save_map), load_map(load_map) + map_file(map_file), load_map(load_map) { // Output welcome message cout << endl << @@ -526,21 +526,32 @@ void System::EnableLocalizationOnly (bool localize_only) { // map serialization addition -void System::SaveMap(const string &filename) { +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; - exit(-1); + return false; } - 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(); + 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; + return false; + } catch (...) { + std::cerr << "Unknows exeption" << std::endl; + return false; + } + + return true; } bool System::LoadMap(const string &filename) { diff --git a/package.xml b/package.xml index fc353331..1646d262 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ tf sensor_msgs dynamic_reconfigure + message_generation roscpp rospy std_msgs @@ -25,6 +26,7 @@ tf sensor_msgs dynamic_reconfigure + message_runtime diff --git a/ros/include/Node.h b/ros/include/Node.h index 0f9eda23..7457748d 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -32,6 +32,8 @@ #include #include +#include "orb_slam2_ros/SaveMap.h" + #include #include #include @@ -61,6 +63,8 @@ class Node void PublishPositionAsPoseStamped(cv::Mat position); void PublishRenderedImage (cv::Mat image); 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); + tf::Transform TransformFromMat (cv::Mat position_mat); sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector map_points); @@ -70,6 +74,8 @@ class Node ros::Publisher map_points_publisher_; ros::Publisher pose_publisher_; + ros::ServiceServer service_server_; + std::string name_of_node_; ros::NodeHandle node_handle_; @@ -78,7 +84,6 @@ class Node std::string map_file_name_param_; std::string voc_file_name_param_; std::string settings_file_name_param_; - bool save_map_param_; bool load_map_param_; bool publish_pointcloud_param_; bool publish_pose_param_; diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 4dc43712..59907352 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -10,7 +10,6 @@ - diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index 1bac88c4..09de55fb 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -11,8 +11,7 @@ - - + diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch index b1fafe30..8028ee96 100644 --- a/ros/launch/orb_slam2_mynteye_s_mono.launch +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -9,7 +9,6 @@ - diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch index 20b1d180..f87e5a7d 100644 --- a/ros/launch/orb_slam2_mynteye_s_stereo.launch +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -11,7 +11,6 @@ - diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch index 8ea02c91..e2646109 100644 --- a/ros/launch/orb_slam2_r200_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -10,7 +10,6 @@ - diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch index d2bc6704..391268f7 100644 --- a/ros/launch/orb_slam2_r200_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -11,7 +11,6 @@ - diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch index 600b4a6c..2394f89e 100644 --- a/ros/launch/orb_slam2_r200_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -11,7 +11,6 @@ - diff --git a/ros/src/Node.cc b/ros/src/Node.cc index d3979161..5eba7f54 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -15,11 +15,11 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima 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_ + "/settings_file", settings_file_name_param_, "file_not_set"); - node_handle_.param(name_of_node_ + "/save_map", save_map_param_, false); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); + orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, load_map_param_); - orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, save_map_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; @@ -46,10 +46,6 @@ Node::~Node () { // Save camera trajectory orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - // Save map - if(save_map_param_) { - orb_slam_->SaveMap(map_file_name_param_); - } delete orb_slam_; } @@ -197,3 +193,14 @@ void Node::ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &confi orb_slam_->SetMinimumKeyFrames (config.min_num_kf_in_map); } + + +bool Node::SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res) { + res.success = orb_slam_->SaveMap(map_file_name_param_); + + if (res.success) { + ROS_INFO ("Map was saved."); + } + + return res.succes; +} \ No newline at end of file diff --git a/srv/SaveMap.srv b/srv/SaveMap.srv new file mode 100644 index 00000000..1308cc21 --- /dev/null +++ b/srv/SaveMap.srv @@ -0,0 +1,2 @@ +--- +bool success \ No newline at end of file From f55ff3ba75417ef67106946c4dd7b80973a4c6de Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 19:22:36 +0200 Subject: [PATCH 38/88] Add documentation for the save and load feature --- README.md | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 9826c211..8235b4eb 100755 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon - Data I/O via ROS topics - 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 ### 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)**. @@ -86,13 +87,17 @@ in your catkin folder. # 3. 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. +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 multiple cameras. If you want to use any other camera you need to adjust this file (you can use one of the provided ones as a template). They are at orb_slam2/config. -## ROS parameters and topics +## ROS parameters, topics and services ### Parameters There are three types of parameters right now: static- and dynamic ros parameters and camera settings from the config file. 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 saved at. +- **settings_file**: String. The location of config file mentioned above. +- **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. - **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. @@ -124,7 +129,14 @@ The following topics are being published and subscribed to by the nodes: - The stereo node subscribes to **image_left/image_color_rect** and - **image_right/image_color_rect** for corresponding images. -# 4. Run +#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 + +# 5. Run After sourcing your setup bash using ``` source devel/setup.bash @@ -138,8 +150,17 @@ source devel/setup.bash Use the command from the corresponding cell for your camera to launch orb_slam2_ros with the right parameters for your setup. -# 5. FAQ +# 6. 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 +rosservice call /orb_slam2_stereo/save_map +rosservice call /orb_slam2_mono/save_map +``` +The file will be saved at ROS_HOME which is by default ~/.ros + ### Using a new / different camera You can use this SLAM with almost any mono, stereo or RGBD cam you want. There are two files which need to be adjusted for a new camera: From 0654b8568ca3cea966c5d13cff485e5e630ae041 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 19:24:47 +0200 Subject: [PATCH 39/88] Fix Headline 4 in the README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8235b4eb..e23157da 100755 --- a/README.md +++ b/README.md @@ -129,7 +129,7 @@ The following topics are being published and subscribed to by the nodes: - The stereo node subscribes to **image_left/image_color_rect** and - **image_right/image_color_rect** for corresponding images. -#4. Services +# 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 From 6cf6dacc5caa580f467ec2e8b9689140a7a75ed5 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 19:26:16 +0200 Subject: [PATCH 40/88] Fix boldness of service names in the README --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index e23157da..be0002b0 100755 --- a/README.md +++ b/README.md @@ -130,11 +130,11 @@ The following topics are being published and subscribed to by the nodes: - **image_right/image_color_rect** for corresponding images. # 4. Services -All nodes offer the possibility to save the map via the service **node_type/save_map**. +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 +- **/orb_slam2_rgbd/save_map** +- **/orb_slam2_mono/save_map** +- **/orb_slam2_stereo/save_map** # 5. Run After sourcing your setup bash using From 8e626c210d9f21991fc14fa6d2a7f27dba0f76bf Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Mon, 15 Jul 2019 19:39:40 +0200 Subject: [PATCH 41/88] Add link to the PR the save-load feature is based on --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index be0002b0..9a703a35 100755 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon - Data I/O via ROS topics - 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 +- Full Map save and load functionality based on [this PR](https://github.com/raulmur/ORB_SLAM2/pull/381). ### 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)**. From 9d1973f98c3bcd75006258330d1edb577cab45f0 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 16 Jul 2019 11:56:11 +0200 Subject: [PATCH 42/88] Fix misspelling of variable --- ros/src/Node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 5eba7f54..58a7a3d9 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -202,5 +202,5 @@ bool Node::SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::Save ROS_INFO ("Map was saved."); } - return res.succes; + return res.success; } \ No newline at end of file From 87286035a7effad160d0afc367646b382baa8386 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Tue, 16 Jul 2019 12:22:19 +0200 Subject: [PATCH 43/88] Implement name specific map_saving --- README.md | 13 +++++++++---- ros/launch/orb_slam2_d435_rgbd.launch | 2 +- ros/src/Node.cc | 6 ++++-- srv/SaveMap.srv | 1 + 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 9a703a35..354d580e 100755 --- a/README.md +++ b/README.md @@ -95,7 +95,7 @@ There are three types of parameters right now: static- and dynamic ros parameter 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 saved at. +- **map_file**: String. The name of the file the map is loaded from. - **settings_file**: String. The location of config file mentioned above. - **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. @@ -136,6 +136,10 @@ So the save_map services are: - **/orb_slam2_mono/save_map** - **/orb_slam2_stereo/save_map** +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 ``` @@ -155,10 +159,11 @@ 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 -rosservice call /orb_slam2_stereo/save_map -rosservice call /orb_slam2_mono/save_map +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 ### Using a new / different camera diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index 09de55fb..d63687f6 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -11,7 +11,7 @@ - + diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 58a7a3d9..aa5d77e4 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -196,10 +196,12 @@ void Node::ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &confi bool Node::SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res) { - res.success = orb_slam_->SaveMap(map_file_name_param_); + res.success = orb_slam_->SaveMap(req.name); if (res.success) { - ROS_INFO ("Map was saved."); + ROS_INFO_STREAM ("Map was saved as " << req.name); + } else { + ROS_ERROR ("Map could not be saved."); } return res.success; diff --git a/srv/SaveMap.srv b/srv/SaveMap.srv index 1308cc21..dc5ebb75 100644 --- a/srv/SaveMap.srv +++ b/srv/SaveMap.srv @@ -1,2 +1,3 @@ +string name --- bool success \ No newline at end of file From d3c2d44c128688d70daadaf8e51f1efaf546404e Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 25 Jul 2019 16:21:25 +0200 Subject: [PATCH 44/88] Fix two segfaults; Save and load works now for large maps! --- orb_slam2/include/System.h | 5 ++++ orb_slam2/src/System.cc | 55 ++++++++++++++++++++++++++++++++++++-- orb_slam2/src/Tracking.cc | 1 + 3 files changed, 59 insertions(+), 2 deletions(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 37148b4f..3eff8969 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -26,6 +26,7 @@ #include #include #include +#include #include "Tracking.h" #include "FrameDrawer.h" @@ -129,6 +130,10 @@ 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(); diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 38f6f580..1eb30a5f 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -497,6 +497,49 @@ 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; @@ -527,7 +570,6 @@ void System::EnableLocalizationOnly (bool localize_only) { // map serialization addition bool System::SaveMap(const string &filename) { - unique_lockMapPointGlobal(MapPoint::mGlobalMutex); std::ofstream out(filename, std::ios_base::binary); if (!out) { @@ -535,6 +577,13 @@ bool System::SaveMap(const string &filename) { 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); @@ -542,15 +591,17 @@ bool System::SaveMap(const string &filename) { 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; } diff --git a/orb_slam2/src/Tracking.cc b/orb_slam2/src/Tracking.cc index 883fecb5..9d8f0dcc 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -1486,6 +1486,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 From c67a6c49f6b05b1b12d8920a1a3e977cb98058a0 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 25 Jul 2019 16:33:39 +0200 Subject: [PATCH 45/88] Make the stereo node compile --- ros/src/StereoNode.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index df409c56..bdad1470 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -25,7 +25,7 @@ int main(int argc, char **argv) } -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); From b507ce88baa34f8dc6d6dbf08328e033dc407262 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 25 Jul 2019 17:47:06 +0200 Subject: [PATCH 46/88] Fix a seqfault which occured when loading large maps --- orb_slam2/src/System.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 1eb30a5f..16df710f 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -614,6 +614,13 @@ bool System::LoadMap(const string &filename) { 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; @@ -638,6 +645,8 @@ bool System::LoadMap(const string &filename) { std::cout << " ... done" << std::endl; in.close(); + + SetCallStackSize(kDefaultCallStackSize); return true; } From e6bb3b65d7b8d620f056e5cad62591b0dd02d853 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 26 Jul 2019 17:49:53 +0200 Subject: [PATCH 47/88] Update README Make it more clear in the README that you need to source the catkin workspace for the custom services to become available --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 354d580e..4401746c 100755 --- a/README.md +++ b/README.md @@ -165,6 +165,7 @@ 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. From 6f64c253780e97a1cfadc7221d1222474d1f40a6 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 26 Jul 2019 17:51:11 +0200 Subject: [PATCH 48/88] Update README New line after the note for the map save-load services in the FAQ section --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 4401746c..17bde300 100755 --- a/README.md +++ b/README.md @@ -165,6 +165,7 @@ 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 From 640569a4af0853b60651f0ecc1916a10722b2c53 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 31 Jul 2019 21:43:59 +0200 Subject: [PATCH 49/88] Add docker file --- Dockerfile | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 Dockerfile diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..305e36a7 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,33 @@ +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 +RUN git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git +RUN git clone https://github.com/IntelRealSense/realsense-ros.git +RUN cd .. + +# 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 -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 python-catkin-tools -y + +# build ros package source +RUN catkin config \ + --extend /opt/ros/$ROS_DISTRO && \ + catkin build From ca869c01e9e2085e26404f5f8a76ba365dc2a0ad Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 31 Jul 2019 22:31:58 +0200 Subject: [PATCH 50/88] Fix the kinetic dockerfile, add a melodic dockerfile and move them in folders --- Dockerfile => docker/kinetic/Dockerfile | 14 ++++++---- docker/melodic/Dockerfile | 37 +++++++++++++++++++++++++ 2 files changed, 46 insertions(+), 5 deletions(-) rename Dockerfile => docker/kinetic/Dockerfile (71%) create mode 100644 docker/melodic/Dockerfile diff --git a/Dockerfile b/docker/kinetic/Dockerfile similarity index 71% rename from Dockerfile rename to docker/kinetic/Dockerfile index 305e36a7..3dee88c2 100644 --- a/Dockerfile +++ b/docker/kinetic/Dockerfile @@ -5,10 +5,10 @@ WORKDIR /home # Get orb_slam_2_ros and the realsense package from git RUN mkdir src -RUN cd src -RUN git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git -RUN git clone https://github.com/IntelRealSense/realsense-ros.git -RUN cd .. +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 @@ -19,7 +19,7 @@ RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3 # Update RUN apt update -RUN apt-get install software-properties-common -y +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 @@ -27,6 +27,10 @@ RUN add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/a # Install required realsense and ROS packages RUN apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg 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 && \ diff --git a/docker/melodic/Dockerfile b/docker/melodic/Dockerfile new file mode 100644 index 00000000..02cd5b7f --- /dev/null +++ b/docker/melodic/Dockerfile @@ -0,0 +1,37 @@ +FROM ros:melodic-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 -y + +#Add realsense repo +RUN sudo 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 install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg 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 From 8abda13cf6fc1c8c53be9a991a63a0dcfcb28ef2 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Wed, 31 Jul 2019 22:38:44 +0200 Subject: [PATCH 51/88] Delete the melodic Dockerfile --- docker/melodic/Dockerfile | 37 ------------------------------------- 1 file changed, 37 deletions(-) delete mode 100644 docker/melodic/Dockerfile diff --git a/docker/melodic/Dockerfile b/docker/melodic/Dockerfile deleted file mode 100644 index 02cd5b7f..00000000 --- a/docker/melodic/Dockerfile +++ /dev/null @@ -1,37 +0,0 @@ -FROM ros:melodic-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 -y - -#Add realsense repo -RUN sudo 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 install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg 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 From 339254f667e147ec66592e0713f05992efd9c7c2 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Thu, 1 Aug 2019 11:21:44 +0200 Subject: [PATCH 52/88] Add some remarks about Docker to the README --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 17bde300..da23dd0f 100755 --- a/README.md +++ b/README.md @@ -154,7 +154,11 @@ source devel/setup.bash Use the command from the corresponding cell for your camera to launch orb_slam2_ros with the right parameters for your setup. -# 6. FAQ +# 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): From e978a2c1d1db6abc93c9a7e0c5bbca01739d8356 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sun, 4 Aug 2019 10:46:42 +0200 Subject: [PATCH 53/88] Add required package for the realsense to the docker file --- docker/kinetic/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/kinetic/Dockerfile b/docker/kinetic/Dockerfile index 3dee88c2..5a71d5c6 100644 --- a/docker/kinetic/Dockerfile +++ b/docker/kinetic/Dockerfile @@ -25,7 +25,7 @@ RUN apt-get install software-properties-common apt-utils -y 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 python-catkin-tools -y +RUN apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg ros-kinetic-rgbd-launch python-catkin-tools -y # Install ROS dependencies RUN rosdep update \ From 36ae984c44d422059bf32c9ac3d3dd3c827717c3 Mon Sep 17 00:00:00 2001 From: Zach Carmichael Date: Sat, 9 Jun 2018 18:18:46 -0400 Subject: [PATCH 54/88] Corrected typedef so that map value_type and allocator are the same (KeyFrameAndPose) --- orb_slam2/include/LoopClosing.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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: From e6cc14a478dc5cb9fa8e536355846b4167c9f88a Mon Sep 17 00:00:00 2001 From: CodesHub <1010914367@qq.com> Date: Thu, 28 Nov 2019 11:47:15 +0800 Subject: [PATCH 55/88] fix maps cannot be saved multiple times in System::SetCallStackSize(), 'rlimit.rlim_cur == kNewStackSize' should not be consider as an erro. --- orb_slam2/src/System.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 16df710f..c34e8f33 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -513,7 +513,7 @@ bool System::SetCallStackSize (const rlim_t kNewStackSize) { return false; } - if (rlimit.rlim_cur < kNewStackSize) { + if (rlimit.rlim_cur <= kNewStackSize) { rlimit.rlim_cur = kNewStackSize; operation_result = setrlimit(RLIMIT_STACK, &rlimit); if (operation_result != 0) { From 9e7dc35d8a6e5e6c16e9c770f86952af5a95c896 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 29 Nov 2019 12:17:07 +0100 Subject: [PATCH 56/88] Fix Camera.bf for D435 --- orb_slam2/config/RealSenseD435RGBD.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orb_slam2/config/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml index 3fe800ac..533401a8 100644 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ b/orb_slam2/config/RealSenseD435RGBD.yaml @@ -17,7 +17,7 @@ Camera.width: 640 Camera.height: 480 # IR projector baseline times fx (aprox.) -Camera.bf: 49.833290 +Camera.bf: 9.052 # Camera frames per second Camera.fps: 30.0 From 95c9e23268cb5f8eb3057948c4ace4d088e9a539 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 29 Nov 2019 12:40:23 +0100 Subject: [PATCH 57/88] Add melodic docker file --- docker/melodic/Dockerfile | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 docker/melodic/Dockerfile diff --git a/docker/melodic/Dockerfile b/docker/melodic/Dockerfile new file mode 100644 index 00000000..2839d1ad --- /dev/null +++ b/docker/melodic/Dockerfile @@ -0,0 +1,37 @@ +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 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 From c47711e2860b961978f94dcf1fe60c55d0d8d694 Mon Sep 17 00:00:00 2001 From: Michael Equi Date: Thu, 26 Mar 2020 20:13:09 -0700 Subject: [PATCH 58/88] added ability to turn off tf publishing --- orb_slam2/include/System.h | 4 ++-- ros/include/Node.h | 1 + ros/src/Node.cc | 12 ++++++++---- ros/src/StereoNode.cc | 4 +++- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 3eff8969..5c85b7d9 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -60,7 +60,7 @@ class System System(const string strVocFile, const string strSettingsFile, const eSensor sensor, 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); @@ -71,7 +71,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); diff --git a/ros/include/Node.h b/ros/include/Node.h index 7457748d..ab20f603 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -86,6 +86,7 @@ class Node std::string settings_file_name_param_; bool load_map_param_; bool publish_pointcloud_param_; + bool publish_tf_param_; bool publish_pose_param_; int min_observations_per_point_; }; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index aa5d77e4..6fad034d 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -10,6 +10,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima //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_ + "/map_file", map_file_name_param_, "map.bin"); @@ -77,9 +78,11 @@ void Node::PublishMapPoints (std::vector map_points) { 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_)); + if(publish_tf_param_){ + 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_)); + } } void Node::PublishPositionAsPoseStamped (cv::Mat position) { @@ -87,7 +90,7 @@ void Node::PublishPositionAsPoseStamped (cv::Mat position) { tf::Stamped grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_); geometry_msgs::PoseStamped pose_msg; tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg); - pose_publisher_.publish(pose_msg); + pose_publisher_.publish(pose_msg); //############################################################################ Turn to with covariance } @@ -107,6 +110,7 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { 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), rotation.at (1,0), rotation.at (1,1), rotation.at (1,2), rotation.at (2,0), rotation.at (2,1), rotation.at (2,2) diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index bdad1470..1ee887d5 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -16,7 +16,7 @@ int main(int argc, char **argv) ros::NodeHandle node_handle; image_transport::ImageTransport image_transport (node_handle); - // initilaize + // initialize StereoNode node (ORB_SLAM2::System::STEREO, node_handle, image_transport); ros::spin(); @@ -59,6 +59,8 @@ void StereoNode::ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft, const } current_frame_time_ = msgLeft->header.stamp; +// ROS_DEBUG("left image time %f", msgLeft->header.stamp.toSec()); +// ROS_DEBUG("right image time: %f", msgRight->header.stamp.toSec()); orb_slam_->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); From 94ed988d1f0d266749eebf95b5b92fe429400240 Mon Sep 17 00:00:00 2001 From: Alden Parker Date: Thu, 26 Mar 2020 22:21:06 -0700 Subject: [PATCH 59/88] building on jetson nano --- CMakeLists.txt | 9 ++++++--- orb_slam2/include/ORBextractor.h | 2 +- orb_slam2/include/PnPsolver.h | 1 + orb_slam2/include/Sim3Solver.h | 1 + orb_slam2/include/Tracking.h | 1 + 5 files changed, 10 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 10412bf0..a4449fa6 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,11 +28,14 @@ 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() diff --git a/orb_slam2/include/ORBextractor.h b/orb_slam2/include/ORBextractor.h index 66e8e7a5..268148a1 100755 --- a/orb_slam2/include/ORBextractor.h +++ b/orb_slam2/include/ORBextractor.h @@ -23,7 +23,7 @@ #include #include -#include +#include namespace ORB_SLAM2 diff --git a/orb_slam2/include/PnPsolver.h b/orb_slam2/include/PnPsolver.h index f92544fc..120f842d 100755 --- a/orb_slam2/include/PnPsolver.h +++ b/orb_slam2/include/PnPsolver.h @@ -52,6 +52,7 @@ #define PNPSOLVER_H #include +#include #include "MapPoint.h" #include "Frame.h" diff --git a/orb_slam2/include/Sim3Solver.h b/orb_slam2/include/Sim3Solver.h index 9af66cb2..b0bdb43f 100755 --- a/orb_slam2/include/Sim3Solver.h +++ b/orb_slam2/include/Sim3Solver.h @@ -23,6 +23,7 @@ #define SIM3SOLVER_H #include +#include #include #include "KeyFrame.h" diff --git a/orb_slam2/include/Tracking.h b/orb_slam2/include/Tracking.h index cbd52fc9..6ef7b4f4 100644 --- a/orb_slam2/include/Tracking.h +++ b/orb_slam2/include/Tracking.h @@ -24,6 +24,7 @@ #include #include +#include #include"FrameDrawer.h" #include"Map.h" From a73bb698887ec0a24505fea03e072c9a36a3c7be Mon Sep 17 00:00:00 2001 From: Michael Equi Date: Fri, 27 Mar 2020 16:45:39 -0700 Subject: [PATCH 60/88] Made more ROS friendly and cleaned up the interface. Still need to implment image resize --- orb_slam2/include/System.h | 13 ++++-- orb_slam2/include/Tracking.h | 18 ++++++++- orb_slam2/src/System.cc | 13 +----- orb_slam2/src/Tracking.cc | 76 ++++++++++++++++++++---------------- ros/include/Node.h | 1 + ros/include/StereoNode.h | 1 - ros/src/Node.cc | 2 +- ros/src/StereoNode.cc | 2 - 8 files changed, 72 insertions(+), 54 deletions(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 5c85b7d9..d20d0647 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -22,12 +22,16 @@ #ifndef SYSTEM_H #define SYSTEM_H -#include -#include +#include +#include #include -#include +#include #include +#include +#include + + #include "Tracking.h" #include "FrameDrawer.h" #include "Map.h" @@ -57,7 +61,7 @@ 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, ros::NodeHandle &node_handle, const std::string & map_file = "", bool load_map = false); // map serialization addition // Process the given stereo frame. Images must be synchronized and rectified. @@ -196,6 +200,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 6ef7b4f4..e46e889d 100644 --- a/orb_slam2/include/Tracking.h +++ b/orb_slam2/include/Tracking.h @@ -26,6 +26,10 @@ #include #include +#include +#include + + #include"FrameDrawer.h" #include"Map.h" #include"LocalMapping.h" @@ -53,7 +57,7 @@ class Tracking public: Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, Map* pMap, - KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); + KeyFrameDatabase* pKFDB, const int sensor, ros::NodeHandle &node_handle); // 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); @@ -213,6 +217,18 @@ class Tracking bool mbRGB; list mlpTemporalPoints; + + + // These parameters are for the ORB features extractor + std::string name_of_node_; + ros::NodeHandle node_handle_; + int nFeatures; + float fScaleFactor; + int nLevels; + int fIniThFAST; + int fMinThFAST; + sensor_msgs::CameraInfo::ConstPtr camera_info; + }; } //namespace ORB_SLAM diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index c34e8f33..1bb7bea7 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -28,7 +28,7 @@ namespace ORB_SLAM2 { -System::System(const string strVocFile, const string strSettingsFile, const eSensor sensor, +System::System(const string strVocFile, const eSensor sensor, ros::NodeHandle &node_handle, 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) @@ -49,15 +49,6 @@ System::System(const string strVocFile, const string strSettingsFile, const eSen 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; @@ -103,7 +94,7 @@ System::System(const string strVocFile, const string strSettingsFile, const eSen //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, mpKeyFrameDatabase, strSettingsFile, mSensor); + mpMap, mpKeyFrameDatabase, mSensor, node_handle); //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); diff --git a/orb_slam2/src/Tracking.cc b/orb_slam2/src/Tracking.cc index 9d8f0dcc..06b886c3 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -43,18 +43,44 @@ using namespace std; namespace ORB_SLAM2 { -Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor): +Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, + Map *pMap, KeyFrameDatabase* pKFDB, const int sensor, ros::NodeHandle &node_handle): mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpFrameDrawer(pFrameDrawer), mpMap(pMap), mnLastRelocFrameId(0), mnMinimumKeyFrames(5) { - // Load camera parameters from settings file + // Load camera parameters + name_of_node_ = ros::this_node::getName(); + node_handle_ = node_handle; + + //ORB SLAM configuration parameters + node_handle_.param(name_of_node_ + "/camera_fps", mMaxFrames, 30); + node_handle_.param(name_of_node_ + "/camera_encoding", mbRGB, true); + node_handle_.param(name_of_node_ + "/ThDepth", mThDepth, static_cast(35.0)); + node_handle_.param(name_of_node_ + "/ORBextractor/nFeatures", nFeatures, 1200); + node_handle_.param(name_of_node_ + "/ORBextractor/scaleFactor", fScaleFactor, static_cast(1.2)); + node_handle_.param(name_of_node_ + "/ORBextractor/nLevels", nLevels, 8); + node_handle_.param(name_of_node_ + "/ORBextractor/iniThFAST", fIniThFAST, 20); + node_handle_.param(name_of_node_ + "/ORBextractor/minThFAST", fMinThFAST, 7); + node_handle_.param(name_of_node_ + "/depth_map_factor", mDepthMapFactor, static_cast(1.0)); + + camera_info = ros::topic::waitForMessage("image_right/camera_info", node_handle_, ros::Duration(10.0)); + if(camera_info == nullptr){ + ROS_ERROR("Did not receive camera info before timeout!"); + throw std::runtime_error("Timeout"); + } + + const float fx = camera_info->K[0]; + const float fy = camera_info->K[4]; + const float cx = camera_info->K[2]; + const float cy = camera_info->K[5]; + + const float k1 = camera_info->D[0]; + const float k2 = camera_info->D[1]; + const float p1 = camera_info->D[2]; + const float p2 = camera_info->D[3]; + const float k3 = camera_info->D[4]; - 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"]; cv::Mat K = cv::Mat::eye(3,3,CV_32F); K.at(0,0) = fx; @@ -64,11 +90,10 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, 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"]; + DistCoef.at(0) = k1; + DistCoef.at(1) = k2; + DistCoef.at(2) = p1; + DistCoef.at(3) = p2; if(k3!=0) { DistCoef.resize(5); @@ -76,15 +101,10 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, } DistCoef.copyTo(mDistCoef); - mbf = fSettings["Camera.bf"]; - - float fps = fSettings["Camera.fps"]; - if(fps==0) - fps=30; + mbf = -camera_info->P[3]; - // 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; @@ -97,25 +117,14 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, 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 +142,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/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 diff --git a/ros/include/Node.h b/ros/include/Node.h index ab20f603..3eb59e6d 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -40,6 +40,7 @@ #include #include #include +#include #include "System.h" diff --git a/ros/include/StereoNode.h b/ros/include/StereoNode.h index b8365b45..fb29a276 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -31,6 +31,5 @@ class StereoNode : public Node message_filters::Subscriber *right_sub_; message_filters::Synchronizer *sync_; - }; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 6fad034d..714e8f20 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -18,7 +18,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima node_handle_.param(name_of_node_ + "/settings_file", settings_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); - orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, settings_file_name_param_, sensor, map_file_name_param_, load_map_param_); + orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor, node_handle_, map_file_name_param_, load_map_param_); service_server_ = node_handle_.advertiseService(name_of_node_+"/save_map", &Node::SaveMapSrv, this); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index 1ee887d5..19e51037 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -59,8 +59,6 @@ void StereoNode::ImageCallback (const sensor_msgs::ImageConstPtr& msgLeft, const } current_frame_time_ = msgLeft->header.stamp; -// ROS_DEBUG("left image time %f", msgLeft->header.stamp.toSec()); -// ROS_DEBUG("right image time: %f", msgRight->header.stamp.toSec()); orb_slam_->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec()); From 241d57615151de14213ecff6a5305bbcd5b39c33 Mon Sep 17 00:00:00 2001 From: Michael Equi Date: Fri, 27 Mar 2020 21:51:02 -0700 Subject: [PATCH 61/88] small refinements --- orb_slam2/src/System.cc | 5 +++++ ros/include/StereoNode.h | 5 +++++ ros/src/StereoNode.cc | 5 ++++- 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 1bb7bea7..c0467c85 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -40,6 +40,11 @@ System::System(const string strVocFile, const eSensor sensor, ros::NodeHandle &n "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) diff --git a/ros/include/StereoNode.h b/ros/include/StereoNode.h index fb29a276..1045e2aa 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -4,6 +4,9 @@ #include #include #include +#include +#include + #include #include @@ -31,5 +34,7 @@ class StereoNode : public Node message_filters::Subscriber *right_sub_; message_filters::Synchronizer *sync_; + int resize_horizontal; + int resize_vertical; }; diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index 19e51037..63719b0d 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -29,6 +29,9 @@ StereoNode::StereoNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle 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); + node_handle.param(ros::this_node::getName()+ "/resize_horizontal", resize_horizontal, 640); + node_handle.param(ros::this_node::getName()+ "/resize_vertical", resize_vertical, 240); + sync_ = new message_filters::Synchronizer (sync_pol(10), *left_sub_, *right_sub_); sync_->registerCallback(boost::bind(&StereoNode::ImageCallback, this, _1, _2)); } @@ -60,7 +63,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 (); } From 84aac90855a57f709c5dabc71b30cc11b98e0415 Mon Sep 17 00:00:00 2001 From: Michael Equi Date: Thu, 9 Apr 2020 20:19:20 -0700 Subject: [PATCH 62/88] Moved ROS stuff out of ORB library and added struct to move parameters to the Tracking class --- orb_slam2/include/System.h | 7 ++-- orb_slam2/include/Tracking.h | 16 +++++--- orb_slam2/src/System.cc | 4 +- orb_slam2/src/Tracking.cc | 76 +++++++++++++----------------------- ros/src/Node.cc | 36 ++++++++++++++++- ros/src/StereoNode.cc | 3 -- 6 files changed, 78 insertions(+), 64 deletions(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index d20d0647..25e47442 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -28,9 +28,6 @@ #include #include -#include -#include - #include "Tracking.h" #include "FrameDrawer.h" @@ -48,6 +45,8 @@ class Tracking; class LocalMapping; class LoopClosing; +struct ORBParameters; + class System { public: @@ -61,7 +60,7 @@ class System public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. - System(const string strVocFile, const eSensor sensor, ros::NodeHandle &node_handle, + System(const string strVocFile, const eSensor sensor, ORBParameters& parameters, const std::string & map_file = "", bool load_map = false); // map serialization addition // Process the given stereo frame. Images must be synchronized and rectified. diff --git a/orb_slam2/include/Tracking.h b/orb_slam2/include/Tracking.h index e46e889d..d5b65045 100644 --- a/orb_slam2/include/Tracking.h +++ b/orb_slam2/include/Tracking.h @@ -52,12 +52,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, - KeyFrameDatabase* pKFDB, const int sensor, ros::NodeHandle &node_handle); + 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); @@ -220,15 +230,11 @@ class Tracking // These parameters are for the ORB features extractor - std::string name_of_node_; - ros::NodeHandle node_handle_; int nFeatures; float fScaleFactor; int nLevels; int fIniThFAST; int fMinThFAST; - sensor_msgs::CameraInfo::ConstPtr camera_info; - }; } //namespace ORB_SLAM diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index c0467c85..f956e19b 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -28,7 +28,7 @@ namespace ORB_SLAM2 { -System::System(const string strVocFile, const eSensor sensor, ros::NodeHandle &node_handle, +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) @@ -99,7 +99,7 @@ System::System(const string strVocFile, const eSensor sensor, ros::NodeHandle &n //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, mpKeyFrameDatabase, mSensor, node_handle); + mpMap, mpKeyFrameDatabase, mSensor, parameters); //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); diff --git a/orb_slam2/src/Tracking.cc b/orb_slam2/src/Tracking.cc index 06b886c3..c15e0137 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -44,73 +44,51 @@ namespace ORB_SLAM2 { Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, - Map *pMap, KeyFrameDatabase* pKFDB, const int sensor, ros::NodeHandle &node_handle): + Map *pMap, 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), mnLastRelocFrameId(0), mnMinimumKeyFrames(5) { - // Load camera parameters - name_of_node_ = ros::this_node::getName(); - node_handle_ = node_handle; - - //ORB SLAM configuration parameters - node_handle_.param(name_of_node_ + "/camera_fps", mMaxFrames, 30); - node_handle_.param(name_of_node_ + "/camera_encoding", mbRGB, true); - node_handle_.param(name_of_node_ + "/ThDepth", mThDepth, static_cast(35.0)); - node_handle_.param(name_of_node_ + "/ORBextractor/nFeatures", nFeatures, 1200); - node_handle_.param(name_of_node_ + "/ORBextractor/scaleFactor", fScaleFactor, static_cast(1.2)); - node_handle_.param(name_of_node_ + "/ORBextractor/nLevels", nLevels, 8); - node_handle_.param(name_of_node_ + "/ORBextractor/iniThFAST", fIniThFAST, 20); - node_handle_.param(name_of_node_ + "/ORBextractor/minThFAST", fMinThFAST, 7); - node_handle_.param(name_of_node_ + "/depth_map_factor", mDepthMapFactor, static_cast(1.0)); - - camera_info = ros::topic::waitForMessage("image_right/camera_info", node_handle_, ros::Duration(10.0)); - if(camera_info == nullptr){ - ROS_ERROR("Did not receive camera info before timeout!"); - throw std::runtime_error("Timeout"); - } - - const float fx = camera_info->K[0]; - const float fy = camera_info->K[4]; - const float cx = camera_info->K[2]; - const float cy = camera_info->K[5]; - - const float k1 = camera_info->D[0]; - const float k2 = camera_info->D[1]; - const float p1 = camera_info->D[2]; - const float p2 = camera_info->D[3]; - const float k3 = camera_info->D[4]; - + //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) = k1; - DistCoef.at(1) = k2; - DistCoef.at(2) = p1; - DistCoef.at(3) = p2; - 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 = -camera_info->P[3]; + mbf = -parameters.baseline; // Max/Min Frames to insert keyframes and to check relocalization mMinFrames = 0; 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) @@ -142,7 +120,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, if(sensor==System::STEREO || sensor==System::RGBD) { - mThDepth = mbf*(float)mThDepth/fx; + mThDepth = mbf*(float)mThDepth/parameters.fx; cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl; } diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 714e8f20..a8385231 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -18,7 +18,41 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima node_handle_.param(name_of_node_ + "/settings_file", settings_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); - orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor, node_handle_, map_file_name_param_, load_map_param_); + // Camera parameters + // Create a parameters object to pass to the Tracking system + 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_encoding", parameters.RGB, true); + node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); + 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); + node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); + + sensor_msgs::CameraInfo::ConstPtr camera_info = ros::topic::waitForMessage("image_right/camera_info", ros::Duration(1000.0)); + if(camera_info == nullptr){ + ROS_ERROR("Did not receive camera info before timeout!"); + throw std::runtime_error("Timeout"); + } + + 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]; + + 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); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index 63719b0d..993094b3 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -29,9 +29,6 @@ StereoNode::StereoNode (const ORB_SLAM2::System::eSensor sensor, ros::NodeHandle 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); - node_handle.param(ros::this_node::getName()+ "/resize_horizontal", resize_horizontal, 640); - node_handle.param(ros::this_node::getName()+ "/resize_vertical", resize_vertical, 240); - sync_ = new message_filters::Synchronizer (sync_pol(10), *left_sub_, *right_sub_); sync_->registerCallback(boost::bind(&StereoNode::ImageCallback, this, _1, _2)); } From e0e18ae06ec27a8a030709a31d1aa644d59dfd21 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 10 Apr 2020 13:59:02 +0200 Subject: [PATCH 63/88] Add the parameters to all launch files; make cam_info optional --- orb_slam2/config/RealSenseD435Mono.yaml | 43 ------- orb_slam2/config/RealSenseD435RGBD.yaml | 52 --------- orb_slam2/config/RealSenseR200Mono.yaml | 43 ------- orb_slam2/config/RealSenseR200RGBD.yaml | 52 --------- orb_slam2/config/RealSenseR200Stereo.yaml | 102 ---------------- orb_slam2/config/TUM1.yaml | 51 -------- orb_slam2/config/mynteye_s_mono.yaml | 63 ---------- orb_slam2/config/mynteye_s_stereo.yaml | 116 ------------------- orb_slam2/config/vimba_stereo.yaml | 48 -------- orb_slam2/src/Tracking.cc | 2 +- ros/include/Node.h | 6 +- ros/launch/orb_slam2_d435_mono.launch | 30 ++++- ros/launch/orb_slam2_d435_rgbd.launch | 35 ++++++ ros/launch/orb_slam2_mynteye_s_mono.launch | 28 +++++ ros/launch/orb_slam2_mynteye_s_stereo.launch | 36 +++++- ros/launch/orb_slam2_r200_mono.launch | 30 ++++- ros/launch/orb_slam2_r200_rgbd.launch | 33 ++++++ ros/launch/orb_slam2_r200_stereo.launch | 33 ++++++ ros/src/MonoNode.cc | 1 + ros/src/Node.cc | 95 ++++++++++----- ros/src/RGBDNode.cc | 1 + ros/src/StereoNode.cc | 1 + 22 files changed, 294 insertions(+), 607 deletions(-) delete mode 100644 orb_slam2/config/RealSenseD435Mono.yaml delete mode 100644 orb_slam2/config/RealSenseD435RGBD.yaml delete mode 100644 orb_slam2/config/RealSenseR200Mono.yaml delete mode 100644 orb_slam2/config/RealSenseR200RGBD.yaml delete mode 100644 orb_slam2/config/RealSenseR200Stereo.yaml delete mode 100644 orb_slam2/config/TUM1.yaml delete mode 100644 orb_slam2/config/mynteye_s_mono.yaml delete mode 100644 orb_slam2/config/mynteye_s_stereo.yaml delete mode 100644 orb_slam2/config/vimba_stereo.yaml diff --git a/orb_slam2/config/RealSenseD435Mono.yaml b/orb_slam2/config/RealSenseD435Mono.yaml deleted file mode 100644 index 86d94db7..00000000 --- a/orb_slam2/config/RealSenseD435Mono.yaml +++ /dev/null @@ -1,43 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 615.6707153320312 -Camera.fy: 615.962158203125 -Camera.cx: 328.0010681152344 -Camera.cy: 241.31031799316406 - -# Camera distortion parameters (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 - -# 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 - -#-------------------------------------------------------------------------------------------- -# 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/RealSenseD435RGBD.yaml b/orb_slam2/config/RealSenseD435RGBD.yaml deleted file mode 100644 index 533401a8..00000000 --- a/orb_slam2/config/RealSenseD435RGBD.yaml +++ /dev/null @@ -1,52 +0,0 @@ -%YAML:1.0 - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 615.6707153320312 -Camera.fy: 615.962158203125 -Camera.cx: 328.0010681152344 -Camera.cy: 241.31031799316406 - -# Camera distortion parameters (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: 9.052 - -# 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: 40.0 - -# Deptmap values factor (what pixel value in the depth image corresponds to 1m?) -DepthMapFactor: 1000.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/RealSenseR200Mono.yaml b/orb_slam2/config/RealSenseR200Mono.yaml deleted file mode 100644 index 36f91d1c..00000000 --- a/orb_slam2/config/RealSenseR200Mono.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/RealSenseR200RGBD.yaml b/orb_slam2/config/RealSenseR200RGBD.yaml deleted file mode 100644 index 6af422c0..00000000 --- a/orb_slam2/config/RealSenseR200RGBD.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: 20.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/RealSenseR200Stereo.yaml b/orb_slam2/config/RealSenseR200Stereo.yaml deleted file mode 100644 index 06d37a86..00000000 --- a/orb_slam2/config/RealSenseR200Stereo.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/mynteye_s_mono.yaml b/orb_slam2/config/mynteye_s_mono.yaml deleted file mode 100644 index 52c521e1..00000000 --- a/orb_slam2/config/mynteye_s_mono.yaml +++ /dev/null @@ -1,63 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters. Adjust them! -#-------------------------------------------------------------------------------------------- - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 332.97713134460906 -Camera.fy: 332.97713134460906 -Camera.cx: 398.9270935058594 -Camera.cy: 252.28187370300293 - -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 - -Camera.width: 752 -Camera.height: 480 - -# Camera frames per second -Camera.fps: 30.0 - -# stereo baseline times fx -Camera.bf: 47.90639384423901 - -# 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: 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 - -#-------------------------------------------------------------------------------------------- -# 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/mynteye_s_stereo.yaml b/orb_slam2/config/mynteye_s_stereo.yaml deleted file mode 100644 index 032b6ee9..00000000 --- a/orb_slam2/config/mynteye_s_stereo.yaml +++ /dev/null @@ -1,116 +0,0 @@ -%YAML:1.0 - -#-------------------------------------------------------------------------------------------- -# Camera Parameters. Adjust them! -#-------------------------------------------------------------------------------------------- - -# Camera calibration and distortion parameters (OpenCV) -Camera.fx: 332.97713134460906 -Camera.fy: 332.97713134460906 -Camera.cx: 398.9270935058594 -Camera.cy: 252.28187370300293 - -Camera.k1: 0.0 -Camera.k2: 0.0 -Camera.p1: 0.0 -Camera.p2: 0.0 - -Camera.width: 752 -Camera.height: 480 - -# Camera frames per second -Camera.fps: 30.0 - -# stereo baseline times fx -Camera.bf: 47.90639384423901 - -# 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: 35 - -#-------------------------------------------------------------------------------------------- -# 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: 480 -LEFT.width: 752 -LEFT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data: [-0.27186431380704884, 0.05397709169334604, -0.000557307377524114, -0.0006127379205397152, 0.0] -LEFT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [360.0652897865692, 0.0, 406.6650580307593, 0.0, 363.2195731683743, 256.20533579714373, 0.0, 0.0, 1.0] -LEFT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [0.9995051975828172, -0.004232452030025982, 0.031168034181628675, 0.00404399044045864, 0.9999731738419173, 0.006107187391924209, -0.031193046440691295, -0.005978122308562103, 0.9994955006939314] -LEFT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [332.97713134460906, 0.0, 398.9270935058594, 0.0, 0.0, 332.97713134460906, 252.28187370300293, 0.0, 0.0, 0.0, 1.0, 0.0] - -RIGHT.height: 480 -RIGHT.width: 752 -RIGHT.D: !!opencv-matrix - rows: 1 - cols: 5 - dt: d - data:[-0.2608724975841786, 0.04949670859088579, -0.00033079602477046385, 0.002888671484465195, 0.0] -RIGHT.K: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [362.3216398160725, 0.0, 356.12189598153714, 0.0, 365.90199497184676, 255.37341022147191, 0.0, 0.0, 1.0] -RIGHT.R: !!opencv-matrix - rows: 3 - cols: 3 - dt: d - data: [0.9971385096122076, -0.004423319050556735, -0.07546672708500173, 0.003966835741299504, 0.9999729264511134, -0.006197626884355805, 0.0754920980139425, 0.005880528324319419, 0.9971290601141259] -RIGHT.P: !!opencv-matrix - rows: 3 - cols: 4 - dt: d - data: [332.97713134460906, 0.0, 398.9270935058594, -39.71778017327359, 0.0, 332.97713134460906, 252.28187370300293, 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 - -#-------------------------------------------------------------------------------------------- -# 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/src/Tracking.cc b/orb_slam2/src/Tracking.cc index c15e0137..0fe9c71d 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -79,7 +79,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, } DistCoef.copyTo(mDistCoef); - mbf = -parameters.baseline; + mbf = parameters.baseline; // Max/Min Frames to insert keyframes and to check relocalization mMinFrames = 0; diff --git a/ros/include/Node.h b/ros/include/Node.h index 3eb59e6d..562add4d 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -55,9 +55,10 @@ class Node 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); @@ -65,6 +66,7 @@ class Node void PublishRenderedImage (cv::Mat image); 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); tf::Transform TransformFromMat (cv::Mat position_mat); sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector map_points); @@ -80,6 +82,8 @@ class Node std::string name_of_node_; ros::NodeHandle node_handle_; + ORB_SLAM2::System::eSensor sensor_; + std::string map_frame_id_param_; std::string camera_frame_id_param_; std::string map_file_name_param_; diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 59907352..612b337e 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -18,5 +18,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index d63687f6..c5101f85 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -19,5 +19,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch index 8028ee96..25798eab 100644 --- a/ros/launch/orb_slam2_mynteye_s_mono.launch +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -17,5 +17,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch index f87e5a7d..9c0f7bfc 100644 --- a/ros/launch/orb_slam2_mynteye_s_stereo.launch +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -19,5 +19,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch index e2646109..b03cbf74 100644 --- a/ros/launch/orb_slam2_r200_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -18,5 +18,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + - \ No newline at end of file + diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch index 391268f7..ce365fa0 100644 --- a/ros/launch/orb_slam2_r200_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -19,5 +19,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch index 2394f89e..09348982 100644 --- a/ros/launch/orb_slam2_r200_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -19,5 +19,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc index 8287feaa..21758922 100644 --- a/ros/src/MonoNode.cc +++ b/ros/src/MonoNode.cc @@ -25,6 +25,7 @@ int main(int argc, char **argv) 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 a8385231..cf0d05eb 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -6,6 +6,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima name_of_node_ = ros::this_node::getName(); node_handle_ = node_handle; min_observations_per_point_ = 2; + sensor_ = sensor; //static parameters node_handle_.param(name_of_node_+ "/publish_pointcloud", publish_pointcloud_param_, true); @@ -18,39 +19,10 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima node_handle_.param(name_of_node_ + "/settings_file", settings_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, false); - // Camera parameters // Create a parameters object to pass to the Tracking system ORB_SLAM2::ORBParameters parameters; + LoadOrbParameters (parameters); - //ORB SLAM configuration parameters - node_handle_.param(name_of_node_ + "/camera_fps", parameters.maxFrames, 30); - node_handle_.param(name_of_node_ + "/camera_encoding", parameters.RGB, true); - node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); - 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); - node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); - - sensor_msgs::CameraInfo::ConstPtr camera_info = ros::topic::waitForMessage("image_right/camera_info", ros::Duration(1000.0)); - if(camera_info == nullptr){ - ROS_ERROR("Did not receive camera info before timeout!"); - throw std::runtime_error("Timeout"); - } - - 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]; orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor, parameters, map_file_name_param_, load_map_param_); @@ -124,7 +96,7 @@ void Node::PublishPositionAsPoseStamped (cv::Mat position) { tf::Stamped grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_); geometry_msgs::PoseStamped pose_msg; tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg); - pose_publisher_.publish(pose_msg); //############################################################################ Turn to with covariance + pose_publisher_.publish(pose_msg); } @@ -243,4 +215,63 @@ bool Node::SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::Save } return res.success; -} \ No newline at end of file +} + + +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 (load_calibration_from_cam) { + 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; + 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 (sensor_==ORB_SLAM2::System::STEREO || sensor_==ORB_SLAM2::System::RGBD) { + node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_baseline", parameters.baseline); + node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); + } + + 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 c33de708..6ff01803 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -27,6 +27,7 @@ int main(int argc, char **argv) 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 993094b3..9678d01f 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -28,6 +28,7 @@ int main(int argc, char **argv) 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)); From 2850189582eab675a3845869b79b718358825d0a Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Fri, 10 Apr 2020 14:07:38 +0200 Subject: [PATCH 64/88] Note in the README that paramters are loaded from launch file --- README.md | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index da23dd0f..1c710c58 100755 --- a/README.md +++ b/README.md @@ -11,6 +11,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon - 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 ### 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)**. @@ -86,8 +87,11 @@ catkin build in your catkin folder. # 3. 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 multiple cameras. If you want to use any other camera you need to adjust this file (you can use one of the provided ones as a template). They are at orb_slam2/config. +## Vocab file +To run the algorithm expects both a vocabulary file (see the paper) which ships with this repository. + +# 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 @@ -155,7 +159,7 @@ source devel/setup.bash 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. +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 @@ -174,8 +178,8 @@ The file will be saved at ROS_HOME which is by default ~/.ros ### Using a new / different camera You can use this SLAM with almost any mono, stereo or RGBD cam you want. -There are two files which need to be adjusted for a new camera: -1) **The yaml config file** at orb_slam2/config for 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 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 need to adjust the other parameters such as Camera.bf, ThDepth and DepthMapFactor. +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. ### Problem running the realsense node From b397a17c6c23cb2914314d8dc8a904dfb2686165 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 11 Apr 2020 16:37:56 +0200 Subject: [PATCH 65/88] Fix bug where params required for rgbd or stereo would load if the cam_info topic was found --- ros/src/Node.cc | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index cf0d05eb..d573264a 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -23,7 +23,6 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima 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); @@ -231,6 +230,12 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { 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)); + got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_baseline", parameters.baseline); + node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); + } + if (load_calibration_from_cam) { sensor_msgs::CameraInfo::ConstPtr camera_info = ros::topic::waitForMessage(camera_info_topic_, ros::Duration(1000.0)); if(camera_info == nullptr){ @@ -263,13 +268,6 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { 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 (sensor_==ORB_SLAM2::System::STEREO || sensor_==ORB_SLAM2::System::RGBD) { - node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); - got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_baseline", parameters.baseline); - node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); - } - if (!got_cam_calibration) { ROS_ERROR ("Failed to get camera calibration parameters from the launch file."); throw std::runtime_error("No cam calibration"); From 8fd3932879fbed83fd6101b46faa61621c2a6896 Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 11 Apr 2020 16:38:30 +0200 Subject: [PATCH 66/88] Add note about the possibility to use cam_info --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 1c710c58..9fff98ac 100755 --- a/README.md +++ b/README.md @@ -12,6 +12,7 @@ This is the ROS implementation of the ORB-SLAM2 real-time SLAM library for **Mon - 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)**. @@ -181,6 +182,7 @@ 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 From 1a38bd1a52f91b5229d4410457cb63403c81eabc Mon Sep 17 00:00:00 2001 From: Lennart Haller Date: Sat, 11 Apr 2020 16:47:41 +0200 Subject: [PATCH 67/88] Fix cam param load logic --- ros/src/Node.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index d573264a..6f10a86b 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -230,9 +230,8 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { 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) { + if (sensor_== ORB_SLAM2::System::STEREO || sensor_==ORB_SLAM2::System::RGBD) { node_handle_.param(name_of_node_ + "/ThDepth", parameters.thDepth, static_cast(35.0)); - got_cam_calibration &= node_handle_.getParam(name_of_node_ + "/camera_baseline", parameters.baseline); node_handle_.param(name_of_node_ + "/depth_map_factor", parameters.depthMapFactor, static_cast(1.0)); } @@ -258,6 +257,10 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { } 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); From 6efc87908e4e18fb1aef3860631fa54c4cb997b5 Mon Sep 17 00:00:00 2001 From: Tim Date: Mon, 13 Apr 2020 18:07:18 +1200 Subject: [PATCH 68/88] Removed unused 'settings_file' param --- README.md | 3 +-- ros/include/Node.h | 1 - ros/launch/orb_slam2_d435_rgbd.launch | 1 - ros/launch/orb_slam2_mynteye_s_mono.launch | 1 - ros/launch/orb_slam2_mynteye_s_stereo.launch | 1 - ros/launch/orb_slam2_r200_mono.launch | 1 - ros/launch/orb_slam2_r200_rgbd.launch | 1 - ros/launch/orb_slam2_r200_stereo.launch | 1 - ros/src/Node.cc | 1 - 9 files changed, 1 insertion(+), 10 deletions(-) diff --git a/README.md b/README.md index 9fff98ac..ac85c0a9 100755 --- a/README.md +++ b/README.md @@ -96,12 +96,11 @@ The config files for camera calibration and tracking hyper paramters from the or ## ROS parameters, topics and services ### Parameters -There are three types of parameters right now: static- and dynamic ros parameters and camera settings from the config file. +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. -- **settings_file**: String. The location of config file mentioned above. - **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. - **publish_pose**: Bool. If a PoseStamped message should be published. Even if this is false the tf will still be published. diff --git a/ros/include/Node.h b/ros/include/Node.h index 562add4d..8d7d595f 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -88,7 +88,6 @@ class Node std::string camera_frame_id_param_; std::string map_file_name_param_; std::string voc_file_name_param_; - std::string settings_file_name_param_; bool load_map_param_; bool publish_pointcloud_param_; bool publish_tf_param_; diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index c5101f85..e0cb1de4 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -13,7 +13,6 @@ - diff --git a/ros/launch/orb_slam2_mynteye_s_mono.launch b/ros/launch/orb_slam2_mynteye_s_mono.launch index 25798eab..c2c65f5c 100644 --- a/ros/launch/orb_slam2_mynteye_s_mono.launch +++ b/ros/launch/orb_slam2_mynteye_s_mono.launch @@ -11,7 +11,6 @@ - diff --git a/ros/launch/orb_slam2_mynteye_s_stereo.launch b/ros/launch/orb_slam2_mynteye_s_stereo.launch index 9c0f7bfc..0b8a43bd 100644 --- a/ros/launch/orb_slam2_mynteye_s_stereo.launch +++ b/ros/launch/orb_slam2_mynteye_s_stereo.launch @@ -13,7 +13,6 @@ - diff --git a/ros/launch/orb_slam2_r200_mono.launch b/ros/launch/orb_slam2_r200_mono.launch index b03cbf74..26fa7440 100644 --- a/ros/launch/orb_slam2_r200_mono.launch +++ b/ros/launch/orb_slam2_r200_mono.launch @@ -12,7 +12,6 @@ - diff --git a/ros/launch/orb_slam2_r200_rgbd.launch b/ros/launch/orb_slam2_r200_rgbd.launch index ce365fa0..6ed07ff9 100644 --- a/ros/launch/orb_slam2_r200_rgbd.launch +++ b/ros/launch/orb_slam2_r200_rgbd.launch @@ -13,7 +13,6 @@ - diff --git a/ros/launch/orb_slam2_r200_stereo.launch b/ros/launch/orb_slam2_r200_stereo.launch index 09348982..de979b11 100644 --- a/ros/launch/orb_slam2_r200_stereo.launch +++ b/ros/launch/orb_slam2_r200_stereo.launch @@ -13,7 +13,6 @@ - diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 6f10a86b..b0168865 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -16,7 +16,6 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima node_handle_.param(name_of_node_+ "/camera_frame_id", camera_frame_id_param_, "camera_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_ + "/settings_file", settings_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 From d79e54fdf8b9b21a6ff025b3cdfc5cc019b00426 Mon Sep 17 00:00:00 2001 From: Tim Date: Tue, 14 Apr 2020 08:16:17 +1200 Subject: [PATCH 69/88] Fix lookup of camera_info --- README.md | 21 ++++++++++++------ ros/include/Node.h | 2 ++ ros/launch/orb_slam2_d435_mono.launch | 1 - ros/src/MonoNode.cc | 2 ++ ros/src/Node.cc | 32 ++++++++++++++------------- ros/src/RGBDNode.cc | 2 ++ ros/src/StereoNode.cc | 2 ++ 7 files changed, 39 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index ac85c0a9..e116d5fd 100755 --- a/README.md +++ b/README.md @@ -106,6 +106,7 @@ The static parameters are send to the ROS parameter server at startup and are no - **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. +- **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: @@ -125,13 +126,19 @@ The following topics are being published and subscribed to by the nodes: - A **tf** from the pointcloud frame id to the camera frame id (the position). ### Subscribed topics -- The mono node subscribes to **/camera/image_raw** for the input image. - -- The RGBD node subscribes to **/camera/rgb/image_raw** for the RGB image and -- **/camera/depth_registered/image_raw** for the depth information. - -- The stereo node subscribes to **image_left/image_color_rect** and -- **image_right/image_color_rect** for corresponding images. +- 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` # 4. Services All nodes offer the possibility to save the map via the service node_type/save_map. diff --git a/ros/include/Node.h b/ros/include/Node.h index 8d7d595f..009fcb6d 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -51,6 +51,7 @@ class Node public: Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport); ~Node (); + void Init (); protected: void Update (); @@ -81,6 +82,7 @@ class Node std::string name_of_node_; ros::NodeHandle node_handle_; + image_transport::ImageTransport image_transport_; ORB_SLAM2::System::eSensor sensor_; diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch index 612b337e..a83cc1df 100644 --- a/ros/launch/orb_slam2_d435_mono.launch +++ b/ros/launch/orb_slam2_d435_mono.launch @@ -12,7 +12,6 @@ - diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc index 21758922..e6e647f3 100644 --- a/ros/src/MonoNode.cc +++ b/ros/src/MonoNode.cc @@ -15,6 +15,8 @@ int main(int argc, char **argv) MonoNode node (ORB_SLAM2::System::MONOCULAR, node_handle, image_transport); + node.Init(); + ros::spin(); ros::shutdown(); diff --git a/ros/src/Node.cc b/ros/src/Node.cc index b0168865..e0368f83 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -2,12 +2,25 @@ #include -Node::Node (ORB_SLAM2::System::eSensor sensor, 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(); 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); @@ -22,7 +35,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima 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_); + 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); @@ -31,7 +44,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2); dynamic_param_server_.setCallback(dynamic_param_callback); - 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); } @@ -40,18 +53,6 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima if (publish_pose_param_) { pose_publisher_ = node_handle_.advertise (name_of_node_+"/pose", 1); } - -} - - -Node::~Node () { - // Stop all threads - orb_slam_->Shutdown(); - - // Save camera trajectory - orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); - - delete orb_slam_; } @@ -235,6 +236,7 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { } if (load_calibration_from_cam) { + ROS_INFO_STREAM ("Listening for camera info on topic " << 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."); diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc index 6ff01803..af76c1fe 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -16,6 +16,8 @@ int main(int argc, char **argv) RGBDNode node (ORB_SLAM2::System::RGBD, node_handle, image_transport); + node.Init(); + ros::spin(); ros::shutdown(); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index 9678d01f..facf7300 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -19,6 +19,8 @@ int main(int argc, char **argv) // initialize StereoNode node (ORB_SLAM2::System::STEREO, node_handle, image_transport); + node.Init(); + ros::spin(); return 0; From 5a8f5b7651203a5876436037dc02405a44b58a8f Mon Sep 17 00:00:00 2001 From: Tim Date: Thu, 16 Apr 2020 18:31:20 +1200 Subject: [PATCH 70/88] show resolved camera info topic when loading orb params --- ros/src/Node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/Node.cc b/ros/src/Node.cc index e0368f83..4c77456a 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -236,7 +236,7 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) { } if (load_calibration_from_cam) { - ROS_INFO_STREAM ("Listening for camera info on topic " << camera_info_topic_); + 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."); From a98e183af86202a858941c864652a3c2165c5ef3 Mon Sep 17 00:00:00 2001 From: Mike Xiangyu Liu Date: Thu, 13 Aug 2020 15:49:42 +0800 Subject: [PATCH 71/88] added TUM2 launch and config file --- orb_slam2/config/TUM2.yaml | 68 +++++++++++++++++++++++++++ ros/launch/orb_slam2_tum2_rgbd.launch | 23 +++++++++ 2 files changed, 91 insertions(+) create mode 100644 orb_slam2/config/TUM2.yaml create mode 100644 ros/launch/orb_slam2_tum2_rgbd.launch 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/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 @@ + + + + + + + + + + + + + + + + + + + + + + From a7b1206cd5119d8df061a62faca54b0244cdcee8 Mon Sep 17 00:00:00 2001 From: Cesar Lopez Date: Wed, 7 Oct 2020 11:05:58 +0200 Subject: [PATCH 72/88] Publish GBA status --- orb_slam2/include/System.h | 3 +++ orb_slam2/src/System.cc | 13 +++++++++---- ros/include/Node.h | 3 +++ ros/src/Node.cc | 9 +++++++++ 4 files changed, 24 insertions(+), 4 deletions(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 25e47442..95ac44bc 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -83,6 +83,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(); diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index f956e19b..45e2454e 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -289,6 +289,11 @@ bool System::MapChanged() return false; } +bool System::isRunningGBA() +{ + return mpLoopCloser->isRunningGBA(); +} + void System::Reset() { unique_lock lock(mMutexReset); @@ -602,7 +607,7 @@ bool System::SaveMap(const string &filename) { } bool System::LoadMap(const string &filename) { - + unique_lockMapPointGlobal(MapPoint::mGlobalMutex); std::ifstream in(filename, std::ios_base::binary); if (!in) { @@ -631,19 +636,19 @@ bool System::LoadMap(const string &filename) { 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; } diff --git a/ros/include/Node.h b/ros/include/Node.h index 009fcb6d..acc3b015 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -41,6 +41,7 @@ #include #include #include +#include #include "System.h" @@ -64,6 +65,7 @@ class Node 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 ParamsChangedCallback(orb_slam2_ros::dynamic_reconfigureConfig &config, uint32_t level); bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res); @@ -77,6 +79,7 @@ class Node image_transport::Publisher rendered_image_publisher_; ros::Publisher map_points_publisher_; ros::Publisher pose_publisher_; + ros::Publisher status_gba_publisher_; ros::ServiceServer service_server_; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 4c77456a..fdf02f51 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -53,6 +53,8 @@ void Node::Init () { 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); } @@ -73,6 +75,8 @@ void Node::Update () { PublishMapPoints (orb_slam_->GetAllMapPoints()); } + PublishGBAStatus (orb_slam_->isRunningGBA()); + } @@ -98,6 +102,11 @@ void Node::PublishPositionAsPoseStamped (cv::Mat position) { 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; From 7282412568e2bfeea8a1eb31b61ce9d386f79c69 Mon Sep 17 00:00:00 2001 From: Albert Seligmann Date: Fri, 8 Jan 2021 12:53:53 +0100 Subject: [PATCH 73/88] Update to use tf2 --- ros/include/Node.h | 5 +++-- ros/src/Node.cc | 41 ++++++++++++++++++++++++++--------------- 2 files changed, 29 insertions(+), 17 deletions(-) diff --git a/ros/include/Node.h b/ros/include/Node.h index 009fcb6d..0d54f333 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -25,10 +25,11 @@ #include #include #include -#include #include #include +#include + #include #include @@ -69,7 +70,7 @@ class Node bool SaveMapSrv (orb_slam2_ros::SaveMap::Request &req, orb_slam2_ros::SaveMap::Response &res); void LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters); - tf::Transform TransformFromMat (cv::Mat position_mat); + tf2::Transform TransformFromMat (cv::Mat position_mat); sensor_msgs::PointCloud2 MapPointsToPointCloud (std::vector map_points); dynamic_reconfigure::Server dynamic_param_server_; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index 4c77456a..e16d433b 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -60,10 +60,12 @@ 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); + PublishPositionAsPoseStamped(position); } } @@ -83,18 +85,27 @@ void Node::PublishMapPoints (std::vector map_points) { void Node::PublishPositionAsTransform (cv::Mat position) { - if(publish_tf_param_){ - 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 message + tf2::Stamped tf_transform_stamped; + tf_transform_stamped = tf2::Stamped(tf_transform, current_frame_time_, map_frame_id_param_); + geometry_msgs::TransformStamped msg = tf2::toMsg(tf_transform_stamped); + msg.child_frame_id = camera_frame_id_param_; + // Broadcast tf + static tf2_ros::TransformBroadcaster tf_broadcaster; + tf_broadcaster.sendTransform(msg); } void Node::PublishPositionAsPoseStamped (cv::Mat position) { - tf::Transform grasp_tf = TransformFromMat (position); - tf::Stamped grasp_tf_pose(grasp_tf, current_frame_time_, map_frame_id_param_); + tf2::Transform tf_position = TransformFromMat(position); + + // Make message + tf2::Stamped tf_position_stamped; + tf_position_stamped = tf2::Stamped(tf_position, current_frame_time_, map_frame_id_param_); geometry_msgs::PoseStamped pose_msg; - tf::poseStampedTFToMsg (grasp_tf_pose, pose_msg); + tf2::toMsg(tf_position_stamped, pose_msg); pose_publisher_.publish(pose_msg); } @@ -108,7 +119,7 @@ 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); @@ -116,15 +127,15 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { 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)); //Coordinate transformation matrix from orb coordinate system to ros coordinate system - const tf::Matrix3x3 tf_orb_to_ros (0, 0, 1, + const tf2::Matrix3x3 tf_orb_to_ros (0, 0, 1, -1, 0, 0, 0,-1, 0); @@ -140,7 +151,7 @@ tf::Transform Node::TransformFromMat (cv::Mat position_mat) { tf_camera_rotation = tf_orb_to_ros*tf_camera_rotation; tf_camera_translation = tf_orb_to_ros*tf_camera_translation; - return tf::Transform (tf_camera_rotation, tf_camera_translation); + return tf2::Transform (tf_camera_rotation, tf_camera_translation); } From f8b5bd2fd037a69ad364db498aa876d12d09974f Mon Sep 17 00:00:00 2001 From: Albert Seligmann Date: Fri, 8 Jan 2021 13:00:18 +0100 Subject: [PATCH 74/88] Add method for transforming map tf and pose estimate to a target frame different from the camera frame --- ros/include/Node.h | 9 ++++++ ros/src/Node.cc | 81 +++++++++++++++++++++++++++++++++++++++++----- 2 files changed, 82 insertions(+), 8 deletions(-) diff --git a/ros/include/Node.h b/ros/include/Node.h index 0d54f333..a166179e 100644 --- a/ros/include/Node.h +++ b/ros/include/Node.h @@ -29,6 +29,9 @@ #include #include +#include +#include +#include #include #include @@ -70,7 +73,12 @@ class Node 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); dynamic_reconfigure::Server dynamic_param_server_; @@ -89,6 +97,7 @@ class Node std::string map_frame_id_param_; std::string camera_frame_id_param_; + std::string target_frame_id_param_; std::string map_file_name_param_; std::string voc_file_name_param_; bool load_map_param_; diff --git a/ros/src/Node.cc b/ros/src/Node.cc index e16d433b..4ccb1932 100644 --- a/ros/src/Node.cc +++ b/ros/src/Node.cc @@ -27,6 +27,7 @@ void Node::Init () { 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); @@ -44,6 +45,10 @@ void Node::Init () { dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2); dynamic_param_server_.setCallback(dynamic_param_callback); + // 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); if (publish_pointcloud_param_) { map_points_publisher_ = node_handle_.advertise (name_of_node_+"/map_points", 1); @@ -60,7 +65,7 @@ void Node::Update () { cv::Mat position = orb_slam_->GetCurrentPosition(); if (!position.empty()) { - if(publish_tf_param_){ + if (publish_tf_param_){ PublishPositionAsTransform(position); } @@ -83,16 +88,73 @@ 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) { // 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_transform_stamped; - tf_transform_stamped = tf2::Stamped(tf_transform, current_frame_time_, map_frame_id_param_); - geometry_msgs::TransformStamped msg = tf2::toMsg(tf_transform_stamped); - msg.child_frame_id = camera_frame_id_param_; + 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); @@ -100,12 +162,15 @@ void Node::PublishPositionAsTransform (cv::Mat position) { 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_stamped; - tf_position_stamped = tf2::Stamped(tf_position, current_frame_time_, map_frame_id_param_); + 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_stamped, pose_msg); + tf2::toMsg(tf_position_target_stamped, pose_msg); pose_publisher_.publish(pose_msg); } From e96990e15d168893a4e0c1a2f5519c56483ec4b6 Mon Sep 17 00:00:00 2001 From: Albert Seligmann Date: Fri, 8 Jan 2021 13:27:07 +0100 Subject: [PATCH 75/88] Add ZED2 launch file --- ros/launch/orb_slam2_zed2_stereo.launch | 66 +++++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 ros/launch/orb_slam2_zed2_stereo.launch diff --git a/ros/launch/orb_slam2_zed2_stereo.launch b/ros/launch/orb_slam2_zed2_stereo.launch new file mode 100644 index 00000000..fd416fee --- /dev/null +++ b/ros/launch/orb_slam2_zed2_stereo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0f1ddc7dfe992e1a1ad8ccf14b169932ef68d673 Mon Sep 17 00:00:00 2001 From: Albert Seligmann Date: Fri, 8 Jan 2021 14:21:52 +0100 Subject: [PATCH 76/88] Add explanation about obtaining camera calibration data --- ros/launch/orb_slam2_zed2_stereo.launch | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ros/launch/orb_slam2_zed2_stereo.launch b/ros/launch/orb_slam2_zed2_stereo.launch index fd416fee..455ae36f 100644 --- a/ros/launch/orb_slam2_zed2_stereo.launch +++ b/ros/launch/orb_slam2_zed2_stereo.launch @@ -47,8 +47,11 @@ - + + + + From 93b5fa79c32b27513e332b6bdc071bb3bdc8dfbf Mon Sep 17 00:00:00 2001 From: aseligmann <40468411+aseligmann@users.noreply.github.com> Date: Sat, 9 Jan 2021 17:47:50 +0100 Subject: [PATCH 77/88] Add ZED 2 (stereo) to supported cameras section --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index e116d5fd..8959b707 100755 --- a/README.md +++ b/README.md @@ -161,7 +161,8 @@ source devel/setup.bash |----------------------|----------------------------------------------------------------|------------------------------------------------------------------|------------------------------------------------------------| | 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 ``` | - | | | | | +| 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. From ae67fff38c9467b8254852e56e7d1f828c0417c5 Mon Sep 17 00:00:00 2001 From: aseligmann <40468411+aseligmann@users.noreply.github.com> Date: Sat, 9 Jan 2021 17:51:08 +0100 Subject: [PATCH 78/88] Add target_frame_id parameter description --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 8959b707..b1452ff9 100755 --- a/README.md +++ b/README.md @@ -106,6 +106,7 @@ The static parameters are send to the ROS parameter server at startup and are no - **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. @@ -121,9 +122,9 @@ Finally, the intrinsic camera calibration parameters along with some hyperparame ### 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. +- 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: From 0357e9e9b2a46713e7b14bd821ddcfeef4b13112 Mon Sep 17 00:00:00 2001 From: JJB_UT <46971781+JackBorer@users.noreply.github.com> Date: Mon, 11 Jan 2021 15:54:45 -0600 Subject: [PATCH 79/88] Update orb_slam2_zed2_stereo.launch Removed references to ipubot_slam and replaced with orb_slam2 --- ros/launch/orb_slam2_zed2_stereo.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros/launch/orb_slam2_zed2_stereo.launch b/ros/launch/orb_slam2_zed2_stereo.launch index 455ae36f..0e7a22cd 100644 --- a/ros/launch/orb_slam2_zed2_stereo.launch +++ b/ros/launch/orb_slam2_zed2_stereo.launch @@ -1,6 +1,6 @@ - + @@ -15,7 +15,7 @@ - + From 1819f3c9a409436f7c7280328815da3a2dc78cdb Mon Sep 17 00:00:00 2001 From: Samuel Laferriere Date: Mon, 22 Mar 2021 22:56:58 -0400 Subject: [PATCH 80/88] Fixed bug (missing tf2-geometry) in Dockerfile and added setup.bash sourcing --- docker/kinetic/Dockerfile | 7 ++++++- docker/melodic/Dockerfile | 6 +++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/docker/kinetic/Dockerfile b/docker/kinetic/Dockerfile index 5a71d5c6..da7266fd 100644 --- a/docker/kinetic/Dockerfile +++ b/docker/kinetic/Dockerfile @@ -25,7 +25,8 @@ RUN apt-get install software-properties-common apt-utils -y 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 python-catkin-tools -y +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 \ @@ -35,3 +36,7 @@ RUN rosdep update \ 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 index 2839d1ad..8fc94b8a 100644 --- a/docker/melodic/Dockerfile +++ b/docker/melodic/Dockerfile @@ -23,7 +23,7 @@ RUN add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/a # Install required realsense and ROS packages RUN apt-get update && \ - apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg python-catkin-tools -y + apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg ros-melodic-tf2-geometry-msgs python-catkin-tools -y WORKDIR /home/ros @@ -35,3 +35,7 @@ RUN rosdep update \ 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 From d62942e3ebda70f7166d5c1e5803a2e3f1452cf3 Mon Sep 17 00:00:00 2001 From: Paulo Eduardo Rodrigues Jr <59392133+PauloRodriguesJr@users.noreply.github.com> Date: Sat, 3 Apr 2021 23:49:23 -0300 Subject: [PATCH 81/88] Fix typo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b1452ff9..0a537323 100755 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ 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 From 544f518f17538972688d86851ca63cd8b9fee5fe Mon Sep 17 00:00:00 2001 From: Caio Amaral Date: Tue, 11 May 2021 09:27:53 -0300 Subject: [PATCH 82/88] Add dependency definition for 'tf2_geometry_msgs' and 'tf2_ros' --- CMakeLists.txt | 2 ++ package.xml | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a4449fa6..76c8a1af 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,8 @@ std_msgs cv_bridge image_transport tf +tf2_geometry_msgs +tf2_ros sensor_msgs dynamic_reconfigure message_generation diff --git a/package.xml b/package.xml index 1646d262..116d9485 100644 --- a/package.xml +++ b/package.xml @@ -9,15 +9,19 @@ GPLv3 catkin + roscpp rospy std_msgs cv_bridge image_transport tf + tf2_geometry_msgs + tf2_ros sensor_msgs dynamic_reconfigure message_generation + roscpp rospy std_msgs From a2da3772302abd9dfa1b7b73f77405b4e5c640d6 Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Thu, 10 Aug 2023 15:02:59 +0100 Subject: [PATCH 83/88] Making it compile on my Ubuntu 20 machine with C++14 --- CMakeLists.txt | 17 ++++++++++------- orb_slam2/include/LoopClosing.h | 2 +- orb_slam2/include/ORBextractor.h | 4 ++-- orb_slam2/include/PnPsolver.h | 4 ++++ orb_slam2/include/Sim3Solver.h | 4 +++- orb_slam2/include/System.h | 4 ++++ ros/include/StereoNode.h | 2 ++ 7 files changed, 26 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 96188769..b9cc9e67 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,11 +9,11 @@ ENDIF() # 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) 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) @@ -29,11 +29,14 @@ pcl_conversions pcl_ros ) -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() 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/ORBextractor.h b/orb_slam2/include/ORBextractor.h index 66e8e7a5..d6ffec7f 100755 --- a/orb_slam2/include/ORBextractor.h +++ b/orb_slam2/include/ORBextractor.h @@ -23,8 +23,8 @@ #include #include -#include - +//#include +#include namespace ORB_SLAM2 { diff --git a/orb_slam2/include/PnPsolver.h b/orb_slam2/include/PnPsolver.h index f92544fc..6bb757ee 100755 --- a/orb_slam2/include/PnPsolver.h +++ b/orb_slam2/include/PnPsolver.h @@ -55,6 +55,10 @@ #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..3ff61443 100755 --- a/orb_slam2/include/Sim3Solver.h +++ b/orb_slam2/include/Sim3Solver.h @@ -27,7 +27,9 @@ #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..3e37c880 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -36,6 +36,10 @@ #include "ORBVocabulary.h" #include "DenseMap.h" +#include +#include +using namespace cv; + namespace ORB_SLAM2 { class FrameDrawer; diff --git a/ros/include/StereoNode.h b/ros/include/StereoNode.h index fed7c7cf..0f47904a 100644 --- a/ros/include/StereoNode.h +++ b/ros/include/StereoNode.h @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include From b53707838c71005a42afb102dc20ce51a6f0cb55 Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Thu, 10 Aug 2023 17:19:32 +0100 Subject: [PATCH 84/88] One more compilation issue - RGBDNode had ambiguous Node that came from Node.h and from cv. --- orb_slam2/include/System.h | 2 +- ros/src/MonoNode.cc | 2 ++ ros/src/RGBDNode.cc | 2 ++ ros/src/StereoNode.cc | 2 ++ 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 3e37c880..28996aea 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -38,7 +38,7 @@ #include #include -using namespace cv; +//using namespace cv; namespace ORB_SLAM2 { diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc index d573f0d4..49dd7f49 100644 --- a/ros/src/MonoNode.cc +++ b/ros/src/MonoNode.cc @@ -1,5 +1,7 @@ #include "MonoNode.h" +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "Mono"); diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc index 796766fb..2c731d70 100644 --- a/ros/src/RGBDNode.cc +++ b/ros/src/RGBDNode.cc @@ -1,5 +1,7 @@ #include "RGBDNode.h" +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "RGBD"); diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc index ee8bb0d4..0cc400b3 100644 --- a/ros/src/StereoNode.cc +++ b/ros/src/StereoNode.cc @@ -4,6 +4,8 @@ #include #include +#include "System.h" + int main(int argc, char **argv) { ros::init(argc, argv, "Stereo"); From 2e4111028028d0f53e186788e243e599075d3322 Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Thu, 10 Aug 2023 17:25:04 +0100 Subject: [PATCH 85/88] Fixed the compilation/linking problems for mt_task_queue. Comments about fixes are in the source files. This gets rid of the issue of 'orb_slam2/lib/liborb_slam2_ros.so: undefined reference to TaskQueue::TaskQueue >, boost::shared_ptr >, boost::shared_ptr >, cv::Mat>::~TaskQueue()' --- .../Thirdparty/mt_task_queue/include/Task.h | 39 +++++++++++-- .../mt_task_queue/include/TaskQueue.h | 33 +++++++++-- .../Thirdparty/mt_task_queue/include/Worker.h | 23 ++++++-- .../Thirdparty/mt_task_queue/src/Task.cpp | 36 ++++++++++-- .../mt_task_queue/src/TaskQueue.cpp | 46 +++++++++++----- .../Thirdparty/mt_task_queue/src/Worker.cpp | 55 ++++++++++++++++--- orb_slam2/src/DenseMap.cc | 3 + 7 files changed, 194 insertions(+), 41 deletions(-) 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..6d1651c6 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_; + /* + * 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..37b35405 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h @@ -23,18 +23,33 @@ 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_;} 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..2a383992 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp @@ -5,43 +5,55 @@ 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 (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++) { + 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 +76,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..83e93386 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp @@ -3,17 +3,45 @@ 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; + queue_mutex_ = queue_mutex; + map_mutex_ = map_mutex; Operator (); } +/** + * 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 () { @@ -22,26 +50,37 @@ void Worker::Operator () { idle_ = true; condition_var_.wait(lock); - queue_mutex_.lock(); + /* + * Since mutexes are now passed as pointers, we need pointers' access operator -> . + */ + queue_mutex_->lock(); if (task_queue_.empty()) { - queue_mutex_.unlock(); + queue_mutex_->unlock(); continue; } idle_ = false; Task top_task = task_queue_.top(); task_queue_.pop(); - queue_mutex_.unlock(); + + queue_mutex_->unlock(); top_task.RunTask (); 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(); } } } +// 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/src/DenseMap.cc b/orb_slam2/src/DenseMap.cc index 28be5f05..d57e49ca 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); } From 4935a0b2d97928a18f9633c1adcc7643ebbe2287 Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Mon, 14 Aug 2023 15:19:39 +0100 Subject: [PATCH 86/88] Fixing threading problems. --- .../mt_task_queue/include/TaskQueue.h | 2 +- .../Thirdparty/mt_task_queue/include/Worker.h | 3 ++ .../mt_task_queue/src/TaskQueue.cpp | 8 ++++-- .../Thirdparty/mt_task_queue/src/Worker.cpp | 28 ++++++++++++++++--- 4 files changed, 33 insertions(+), 8 deletions(-) diff --git a/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h b/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h index 6d1651c6..49ace5bb 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/TaskQueue.h @@ -56,7 +56,7 @@ class TaskQueue { private: std::priority_queue , std::vector>, std::greater>> task_queue_; - std::vector> workers_; + 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. */ diff --git a/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h b/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h index 37b35405..99253892 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h +++ b/orb_slam2/Thirdparty/mt_task_queue/include/Worker.h @@ -39,6 +39,9 @@ class Worker { void EndWorker() {end_operator_flag_ = true;} bool IsIdeling () {return idle_;} + ~Worker(); + + private: std::priority_queue , std::vector>, std::greater>> &task_queue_; /* diff --git a/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp b/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp index 2a383992..ee06b67f 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/TaskQueue.cpp @@ -9,11 +9,12 @@ TaskQueue::TaskQueue (unsigned int num_worker_threads) { } for (unsigned int i = 0; i < num_worker_threads; i++) { - workers_.push_back (Worker (task_queue_, results_, &queue_mutex_, &map_mutex_, condition_var_)); + workers_.push_back (new Worker (task_queue_, results_, &queue_mutex_, &map_mutex_, condition_var_)); } } + template TaskQueue::~TaskQueue () { /* @@ -21,7 +22,8 @@ TaskQueue::~TaskQueue () { * out of the queue, which would cause copy-construction. */ for (unsigned int i = 0; i < workers_.size(); i++) { - workers_[i].EndWorker(); + //std::cout << "AE: TQ DESTR" << std::endl; + workers_[i]->EndWorker(); } } @@ -81,7 +83,7 @@ unsigned int TaskQueue::NumJobsCurrentlyRunning () { * 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??? + if (!workers_[i]->IsIdeling()) { //# if (workers_[i].IsIdeling()) { //# surely we want to count workers that are NOT idle though??? workers_working ++; } } diff --git a/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp b/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp index 83e93386..47c82b2e 100644 --- a/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp +++ b/orb_slam2/Thirdparty/mt_task_queue/src/Worker.cpp @@ -17,7 +17,19 @@ Worker::Worker (std::priority_queue +Worker::~Worker () { + std::cout << "AE: WORKER DESTR" << std::endl; } /** @@ -49,23 +61,26 @@ void Worker::Operator () { while (!end_operator_flag_) { idle_ = true; condition_var_.wait(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(); 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(); - + 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()) { /* @@ -75,8 +90,13 @@ void Worker::Operator () { 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 From c8f75390847089425cadae072a3420e81b4fc93d Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Mon, 14 Aug 2023 16:55:32 +0100 Subject: [PATCH 87/88] Adding debug statements --- orb_slam2/src/DenseMap.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/orb_slam2/src/DenseMap.cc b/orb_slam2/src/DenseMap.cc index d57e49ca..d121b8b5 100644 --- a/orb_slam2/src/DenseMap.cc +++ b/orb_slam2/src/DenseMap.cc @@ -53,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; } From a6e41047aaec6ac26ee6ca8704b0064e91740de8 Mon Sep 17 00:00:00 2001 From: Arturs Elksnis Date: Mon, 14 Aug 2023 23:09:53 +0100 Subject: [PATCH 88/88] Changed the launch file for the correct topic --- ros/launch/orb_slam2_d435_rgbd.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros/launch/orb_slam2_d435_rgbd.launch b/ros/launch/orb_slam2_d435_rgbd.launch index e0cb1de4..401127af 100644 --- a/ros/launch/orb_slam2_d435_rgbd.launch +++ b/ros/launch/orb_slam2_d435_rgbd.launch @@ -2,7 +2,8 @@ - + +