diff --git a/README.md b/README.md index b1632adc..4556e02c 100644 --- a/README.md +++ b/README.md @@ -56,18 +56,26 @@ We make use of some components of the DBoW2 library for place recognition and fe including only the components we need and also some modifications that are listed in `Thirdparty/DBoW2/LICENSE.txt`. It only depends on OpenCV, but it should be included in the ROS distribution. +##2.5 Octomap +The integrated octomap export topics und services require following packages. + + sudo apt-get install ros-indigo-octomap ros-indigo-octomap-msgs ros-indigo-octomap-ros #3. Installation 1. In your ROS package path (check your environment variable `ROS_PACKAGE_PATH`) clone this repository: + Original version: git clone https://github.com/raulmur/ORB_SLAM.git ORB_SLAM + My version: + git clone https://github.com/cehberlin/ORB_SLAM.git 2. Run this line from your catkin workspace root, `indigo` here should be replaced with your preferred ROS distro. `rosdep install --from-paths src --ignore-src --rosdistro indigo -y` 3. Build all by running catkin_make in your workspace root. + Only original version: *Tip: Set your favorite compilation flags in line 12 and 13 of* `Thirdparty/DBoW2/CMakeLists.txt` (by default -03 -march=native) #4. Usage @@ -79,7 +87,7 @@ It only depends on OpenCV, but it should be included in the ROS distribution. rosrun orb_slam orb_slam PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE You have to provide the path to the ORB vocabulary and to the settings file. The paths must be absolute or relative to the orb_slam directory. - We already provide the vocabulary file we use in `orb_slam/Data/ORBvoc.yml`. Uncompress the file, as it will be loaded much faster. + We already provide the vocabulary file we use in `orb_slam/Data/ORBvoc.txt`. Uncompress the file, as it will be loaded much faster. 2. The last processed frame is published to the topic `/orb_slam/Frame`. You can visualize it using `image_view`: @@ -124,7 +132,7 @@ We provide the settings and the rosbag of an example sequence in our lab. In thi Uncompress the file. -2. Launch ORB_SLAM with the settings for the example sequence. You should have already uncompressed the vocabulary file (`/Data/ORBvoc.yml.tar.gz`) +2. Launch ORB_SLAM with the settings for the example sequence. You should have already uncompressed the vocabulary file (`/Data/ORBvoc.txt.tar.gz`) *in ROS Fuerte*: diff --git a/g2o/CMakeLists.txt b/g2o/CMakeLists.txt index 683c9877..0eea2f9b 100644 --- a/g2o/CMakeLists.txt +++ b/g2o/CMakeLists.txt @@ -109,8 +109,8 @@ IF(OPENMP_FOUND AND G2O_USE_OPENMP) ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP) # Compiler specific options for gcc -SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native") -SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native") +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 ") +SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 ") # activate warnings !!! SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W") diff --git a/orb_slam/CMakeLists.txt b/orb_slam/CMakeLists.txt index 2ba92e5e..e3c2407c 100644 --- a/orb_slam/CMakeLists.txt +++ b/orb_slam/CMakeLists.txt @@ -9,15 +9,39 @@ find_package(catkin REQUIRED COMPONENTS image_geometry cv_bridge pcl_ros + pcl_conversions g2o + message_generation ) +ADD_DEFINITIONS("-std=c++11") + #LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) #find_package(Cholmod REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) +find_package(octomap 1.6 REQUIRED) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + ORBState.msg +) + +add_service_files( + FILES + SaveOctomap.srv +) -catkin_package() +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs + DEPENDS message_runtime std_msgs +) message(*************************) message(${catkin_INCLUDE_DIRS}) @@ -31,10 +55,11 @@ include_directories( ${PCL_INCLUDE_DIRS} ${CHOLMOD_INCLUDE_DIR} ${PROJECT_SOURCE_DIR} + ${OCTOMAP_INCLUDE_DIRS} ) -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 ") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 ") add_subdirectory(Thirdparty/DBoW2) @@ -51,16 +76,22 @@ add_executable(${PROJECT_NAME} src/KeyFrame.cc src/Map.cc src/MapPublisher.cc + src/OctoMapPublisher.cc src/Optimizer.cc src/PnPsolver.cc src/Frame.cc src/KeyFrameDatabase.cc src/Sim3Solver.cc + src/StatePublisher.cc src/Initializer.cc ) + +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) + target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${EIGEN3_LIBS} + ${OCTOMAP_LIBRARIES} # cholmod DBoW2 # g2o_core diff --git a/orb_slam/Data/ORBvoc.yml.tar.gz b/orb_slam/Data/ORBvoc.txt.tar.gz similarity index 67% rename from orb_slam/Data/ORBvoc.yml.tar.gz rename to orb_slam/Data/ORBvoc.txt.tar.gz index bf336dae..1e3cac9c 100644 Binary files a/orb_slam/Data/ORBvoc.yml.tar.gz and b/orb_slam/Data/ORBvoc.txt.tar.gz differ diff --git a/orb_slam/Data/Settings_Morse_Kinect.yaml b/orb_slam/Data/Settings_Morse_Kinect.yaml new file mode 100644 index 00000000..aa69f1e3 --- /dev/null +++ b/orb_slam/Data/Settings_Morse_Kinect.yaml @@ -0,0 +1,46 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# 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 + +# Camera calibration parameters (OpenCV) +Camera.fx: 700 +Camera.fy: 700 +Camera.cx: 320 +Camera.cy: 240 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: 0 +Camera.k2: 0 +Camera.p1: 0 +Camera.p2: 0 + +# Are the images already rectified? 0=no rectified, 1=rectified +Camera.Rectified: 0 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 20 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 1 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_Morse_Videocam.yaml b/orb_slam/Data/Settings_Morse_Videocam.yaml new file mode 100644 index 00000000..d6d1f822 --- /dev/null +++ b/orb_slam/Data/Settings_Morse_Videocam.yaml @@ -0,0 +1,46 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# 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 + +# Camera calibration parameters (OpenCV) +Camera.fx: 200 +Camera.fy: 200 +Camera.cx: 320 +Camera.cy: 240 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: 0 +Camera.k2: 0 +Camera.p1: 0 +Camera.p2: 0 + +# Are the images already rectified? 0=no rectified, 1=rectified +Camera.Rectified: 0 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 20 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 1 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_bluefox_2mm.yaml b/orb_slam/Data/Settings_bluefox_2mm.yaml new file mode 100644 index 00000000..cf3aa857 --- /dev/null +++ b/orb_slam/Data/Settings_bluefox_2mm.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 343.994948 +Camera.fy: 343.795767 +Camera.cx: 380.994941 +Camera.cy: 259.645375 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: -0.226983 +Camera.k2: 0.033991 +Camera.p1: 0.001966 +Camera.p2: -0.001217 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 10 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 0 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_bluefox_2mm_stephan.yaml b/orb_slam/Data/Settings_bluefox_2mm_stephan.yaml new file mode 100644 index 00000000..7f7c71ee --- /dev/null +++ b/orb_slam/Data/Settings_bluefox_2mm_stephan.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 362.731416 +Camera.fy: 363.585359 +Camera.cx: 377.087595 +Camera.cy: 273.554744 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: -0.307447 +Camera.k2: 0.072806 +Camera.p1: 0.001345 +Camera.p2: -0.001264 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 10 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 0 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_bluefox_6mm.yaml b/orb_slam/Data/Settings_bluefox_6mm.yaml new file mode 100644 index 00000000..59126f26 --- /dev/null +++ b/orb_slam/Data/Settings_bluefox_6mm.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 1014.270444 +Camera.fy: 1011.963701 +Camera.cx: 365.890443 +Camera.cy: 278.257872 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: -0.611255 +Camera.k2: 0.427913 +Camera.p1: -0.001147 +Camera.p2: 0.000403 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 10 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 0 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_bluefox_6mm_fuji.yaml b/orb_slam/Data/Settings_bluefox_6mm_fuji.yaml new file mode 100644 index 00000000..967fed4f --- /dev/null +++ b/orb_slam/Data/Settings_bluefox_6mm_fuji.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 1002.225016 +Camera.fy: 1001.360963 +Camera.cx: 383.976128 +Camera.cy: 247.381942 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: -0.173559 +Camera.k2: -0.007583 +Camera.p1: -0.002389 +Camera.p2: 0.001295 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 10 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 0 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_hq30_320_240.yaml b/orb_slam/Data/Settings_hq30_320_240.yaml new file mode 100644 index 00000000..e0a8d53d --- /dev/null +++ b/orb_slam/Data/Settings_hq30_320_240.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 426.847106 +Camera.fy: 425.303251 +Camera.cx: 160.922767 +Camera.cy: 130.718021 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: 0.201790 +Camera.k2: -0.801727 +Camera.p1: 0.002269 +Camera.p2: 0.001058 + +# 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: 0 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 500 + +# 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 (lower less restrictive) +ORBextractor.fastTh: 20 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 1 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_logitech_hd_320_240.yaml b/orb_slam/Data/Settings_logitech_hd_320_240.yaml new file mode 100644 index 00000000..95c5ba04 --- /dev/null +++ b/orb_slam/Data/Settings_logitech_hd_320_240.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 317.587260 +Camera.fy: 317.234902 +Camera.cx: 162.139122 +Camera.cy: 125.841956 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: 0.138146 +Camera.k2: -0.249315 +Camera.p1: 0.001882 +Camera.p2: -0.001033 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 500 + +# 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 (lower less restrictive) +ORBextractor.fastTh: 20 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 1 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/Data/Settings_logitech_hd_640_480.yaml b/orb_slam/Data/Settings_logitech_hd_640_480.yaml new file mode 100644 index 00000000..6736041f --- /dev/null +++ b/orb_slam/Data/Settings_logitech_hd_640_480.yaml @@ -0,0 +1,43 @@ +%YAML:1.0 + +# Camera Parameters. Adjust them! + +# Camera calibration parameters (OpenCV) +Camera.fx: 846.866781 +Camera.fy: 848.355478 +Camera.cx: 340.012838 +Camera.cy: 269.567734 + +# Camera distortion paremeters (OpenCV) -- +Camera.k1: 0.210913 +Camera.k2: -0.680321 +Camera.p1: 0.005027 +Camera.p2: 0.008679 + +# 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 + +#-------------------------------------------------------------------------------------------- +### Changing the parameters below could seriously degradate the performance of the system + +# 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 (lower less restrictive) +ORBextractor.fastTh: 10 + +# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score +ORBextractor.nScoreType: 0 + + +# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) +UseMotionModel: 1 diff --git a/orb_slam/README.txt b/orb_slam/README.txt index ae69444c..77f1cca1 100644 --- a/orb_slam/README.txt +++ b/orb_slam/README.txt @@ -129,6 +129,16 @@ Tip: Use a roslaunch to launch ORB_SLAM, image_view and rviz from just one instr roslaunch ExampleGroovyHydro.launch +5. In order to get useful results from the OctoMapPublisher module you need the transformations as shown in the following picture + +![ORB_SLAM OctoMapPublisher TFs](https://github.com/cehberlin/ORB_SLAM/tree/indigo-devel/orb_slam/doc/map_tfs.png "TFs needed for creating maps") + +* ORB_SLAM/Camera is the localisation from the SLAM algorithm itself +* ORB_base_link is the planar base of your system relative to the (tilted) camera, e.g. the robot platform +* ORB_SLAM/World is the initial localisation of the SLAM in the camera coordinate frame +* orb_slam/map is the back transformation from the world camera frame at the momement of initialisation into the map coordinate frame (a planar frame) +* world is the transformation + 5) Example Sequence We provide the settings and the rosbag of an example sequence in our lab. In this sequence you will see a loop closure and two relocalisation from a big viewpoint change. diff --git a/orb_slam/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/orb_slam/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index d8cf97b5..6e282b57 100644 --- a/orb_slam/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/orb_slam/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -1,3 +1,10 @@ +/** + * This is a modified version of TemplatedVocabulary.h from DBoW2 (see below). + * Added functions: Save and Load from text files without using cv::FileStorage. + * Date: August 2015 + * Raúl Mur-Artal + */ + /** * File: TemplatedVocabulary.h * Date: February 2011 @@ -18,11 +25,11 @@ #include #include #include +#include #include "FeatureVector.h" #include "BowVector.h" #include "ScoringObject.h" -#include #include "../DUtils/Random.h" @@ -226,7 +233,19 @@ class TemplatedVocabulary * @param type new scoring type */ void setScoringType(ScoringType type); - + + /** + * Loads the vocabulary from a text file + * @param filename + */ + bool loadFromTextFile(const std::string &filename); + + /** + * Saves the vocabulary into a text file + * @param filename + */ + void saveToTextFile(const std::string &filename) const; + /** * Saves the vocabulary into a file * @param filename @@ -443,7 +462,7 @@ TemplatedVocabulary::TemplatedVocabulary template void TemplatedVocabulary::createScoringObject() { - //delete m_scoring_object; + delete m_scoring_object; m_scoring_object = NULL; switch(m_scoring) @@ -507,7 +526,7 @@ TemplatedVocabulary::TemplatedVocabulary( template TemplatedVocabulary::~TemplatedVocabulary() { - //delete m_scoring_object; + delete m_scoring_object; } // -------------------------------------------------------------------------- @@ -1315,6 +1334,116 @@ int TemplatedVocabulary::stopWords(double minWeight) // -------------------------------------------------------------------------- +template +bool TemplatedVocabulary::loadFromTextFile(const std::string &filename) +{ + ifstream f; + f.open(filename.c_str()); + + if(f.eof()) + return false; + + m_words.clear(); + m_nodes.clear(); + + string s; + getline(f,s); + stringstream ss; + ss << s; + ss >> m_k; + ss >> m_L; + int n1, n2; + ss >> n1; + m_scoring = (ScoringType)n1; + ss >> n2; + m_weighting = (WeightingType)n2; + createScoringObject(); + + if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3) + { + std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl; + return false; + } + + // nodes + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); + m_nodes.reserve(expected_nodes); + + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + + m_nodes.resize(1); + m_nodes[0].id = 0; + while(!f.eof()) + { + string snode; + getline(f,snode); + stringstream ssnode; + ssnode << snode; + + int nid = m_nodes.size(); + m_nodes.resize(m_nodes.size()+1); + + int pid ; + ssnode >> pid; + m_nodes[nid].parent = pid; + m_nodes[pid].children.push_back(nid); + + int nIsLeaf; + ssnode >> nIsLeaf; + + string sd; + for(int iD=0;iD> sd; + F::fromString(m_nodes[nid].descriptor, sd); + + ssnode >> m_nodes[nid].weight; + + if(nIsLeaf>0) + { + int wid = m_words.size(); + m_words.resize(wid+1); + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } + else + { + m_nodes[nid].children.reserve(m_k); + } + } + + return true; + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::saveToTextFile(const std::string &filename) const +{ + fstream f; + f.open(filename.c_str(),ios_base::out); + f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl; + + for(size_t i=1; i void TemplatedVocabulary::save(const std::string &filename) const { diff --git a/orb_slam/doc/map_tfs.png b/orb_slam/doc/map_tfs.png new file mode 100644 index 00000000..ab983bca Binary files /dev/null and b/orb_slam/doc/map_tfs.png differ diff --git a/orb_slam/doc/map_tfs_raw.png b/orb_slam/doc/map_tfs_raw.png new file mode 100644 index 00000000..b0539237 Binary files /dev/null and b/orb_slam/doc/map_tfs_raw.png differ diff --git a/orb_slam/include/Frame.h b/orb_slam/include/Frame.h index 3de40598..a0814eb3 100644 --- a/orb_slam/include/Frame.h +++ b/orb_slam/include/Frame.h @@ -21,6 +21,8 @@ #ifndef FRAME_H #define FRAME_H +#include + #include "MapPoint.h" #include "Thirdparty/DBoW2/DBoW2/BowVector.h" #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" @@ -45,12 +47,12 @@ class Frame public: Frame(); Frame(const Frame &frame); - Frame(cv::Mat &im, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef); + Frame(cv::Mat &im, const double &timeStamp, std::shared_ptr extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef); ORBVocabulary* mpORBvocabulary; - ORBextractor* mpORBextractor; + std::shared_ptr mpORBextractor; - // Mono image + // Frame image cv::Mat im; // Frame timestamp @@ -79,14 +81,14 @@ class Frame cv::Mat mDescriptors; // MapPoints associated to keypoints, NULL pointer if not association - std::vector mvpMapPoints; + std::vector> mvpMapPoints; // Flag to identify outlier associations std::vector mvbOutlier; // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints - float mfGridElementWidthInv; - float mfGridElementHeightInv; + static float mfGridElementWidthInv; + static float mfGridElementHeightInv; std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; // Camera Pose @@ -96,14 +98,14 @@ class Frame static long unsigned int nNextId; long unsigned int mnId; - KeyFrame* mpReferenceKF; + std::shared_ptr mpReferenceKF; void ComputeBoW(); void UpdatePoseMatrices(); // Check if a MapPoint is in the frustum of the camera and also fills variables of the MapPoint to be used by the tracking - bool isInFrustum(MapPoint* pMP, float viewingCosLimit); + bool isInFrustum(std::shared_ptr pMP, float viewingCosLimit); // Compute the cell of a keypoint (return false if outside the grid) bool PosInGrid(cv::KeyPoint &kp, int &posX, int &posY); @@ -128,7 +130,7 @@ class Frame private: - void UndistortKeyPoints(const std::vector &kps, std::vector &ukps); + void UndistortKeyPoints(); void ComputeImageBounds(); // Call UpdatePoseMatrices(), before using diff --git a/orb_slam/include/FramePublisher.h b/orb_slam/include/FramePublisher.h index 2b7cdd94..6f455ee2 100644 --- a/orb_slam/include/FramePublisher.h +++ b/orb_slam/include/FramePublisher.h @@ -41,7 +41,7 @@ class Tracking; class FramePublisher { public: - FramePublisher(); + FramePublisher(); void Update(Tracking *pTracker); @@ -62,7 +62,7 @@ class FramePublisher vector mvbOutliers; - vector mvpMatchedMapPoints; + vector> mvpMatchedMapPoints; int mnTracked; vector mvIniKeys; vector mvIniMatches; diff --git a/orb_slam/include/Initializer.h b/orb_slam/include/Initializer.h index 1dc383f5..61a4b5fa 100644 --- a/orb_slam/include/Initializer.h +++ b/orb_slam/include/Initializer.h @@ -21,7 +21,7 @@ #ifndef INITIALIZER_H #define INITIALIZER_H -#include +#include #include "Frame.h" @@ -92,7 +92,7 @@ class Initializer int mMaxIterations; // Ransac sets - vector > mvSets; + vector > mvSets; }; diff --git a/orb_slam/include/KeyFrame.h b/orb_slam/include/KeyFrame.h index ff4586bb..d6a04d9f 100644 --- a/orb_slam/include/KeyFrame.h +++ b/orb_slam/include/KeyFrame.h @@ -29,7 +29,8 @@ #include "Frame.h" #include "KeyFrameDatabase.h" -#include +#include +#include namespace ORB_SLAM @@ -41,7 +42,7 @@ class Frame; class KeyFrameDatabase; class DatabaseResult; -class KeyFrame +class KeyFrame: public std::enable_shared_from_this { public: KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); @@ -65,37 +66,37 @@ class KeyFrame DBoW2::BowVector GetBowVector(); // Covisibility graph functions - void AddConnection(KeyFrame* pKF, const int &weight); - void EraseConnection(KeyFrame* pKF); + void AddConnection(std::shared_ptr pKF, const int &weight); + void EraseConnection(std::shared_ptr pKF); void UpdateConnections(); void UpdateBestCovisibles(); - std::set GetConnectedKeyFrames(); - std::vector GetVectorCovisibleKeyFrames(); - std::vector GetBestCovisibilityKeyFrames(const int &N); - std::vector GetCovisiblesByWeight(const int &w); - int GetWeight(KeyFrame* pKF); + std::set> GetConnectedKeyFrames(); + std::vector > GetVectorCovisibleKeyFrames(); + std::vector> GetBestCovisibilityKeyFrames(const int &N); + std::vector> GetCovisiblesByWeight(const int &w); + int GetWeight(std::shared_ptr pKF); // Spanning tree functions - void AddChild(KeyFrame* pKF); - void EraseChild(KeyFrame* pKF); - void ChangeParent(KeyFrame* pKF); - std::set GetChilds(); - KeyFrame* GetParent(); - bool hasChild(KeyFrame* pKF); + void AddChild(std::shared_ptr pKF); + void EraseChild(std::shared_ptr pKF); + void ChangeParent(std::shared_ptr pKF); + std::set> GetChilds(); + std::shared_ptr GetParent(); + bool hasChild(std::shared_ptr pKF); // Loop Edges - void AddLoopEdge(KeyFrame* pKF); - std::set GetLoopEdges(); + void AddLoopEdge(std::shared_ptr pKF); + std::set> GetLoopEdges(); // MapPoint observation functions - void AddMapPoint(MapPoint* pMP, const size_t &idx); + void AddMapPoint(std::shared_ptr pMP, const size_t &idx); void EraseMapPointMatch(const size_t &idx); - void EraseMapPointMatch(MapPoint* pMP); - void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP); - std::set GetMapPoints(); - std::vector GetMapPointMatches(); + void EraseMapPointMatch(std::shared_ptr pMP); + void ReplaceMapPointMatch(const size_t &idx, std::shared_ptr pMP); + std::set> GetMapPoints(); + std::vector> GetMapPointMatches(); int TrackedMapPoints(); - MapPoint* GetMapPoint(const size_t &idx); + std::shared_ptr GetMapPoint(const size_t &idx); // KeyPoint functions cv::KeyPoint GetKeyPointUn(const size_t &idx) const; @@ -174,7 +175,7 @@ class KeyFrame return a>b; } - static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){ + static bool lId(std::shared_ptr pKF1, std::shared_ptr pKF2){ return pKF1->mnIdmnId; } @@ -193,11 +194,11 @@ class KeyFrame int mnMaxY; cv::Mat mK; - // KeyPoints, Descriptors, Points3D, MapPoints vectors (all associated by an index) + // KeyPoints, Descriptors, MapPoints vectors (all associated by an index) std::vector mvKeys; std::vector mvKeysUn; cv::Mat mDescriptors; - std::vector mvpMapPoints; + std::vector> mvpMapPoints; // BoW KeyFrameDatabase* mpKeyFrameDB; @@ -208,15 +209,15 @@ class KeyFrame // Grid over the image to speed up feature matching std::vector< std::vector > > mGrid; - std::map mConnectedKeyFrameWeights; - std::vector mvpOrderedConnectedKeyFrames; + std::map,int> mConnectedKeyFrameWeights; + std::vector> mvpOrderedConnectedKeyFrames; std::vector mvOrderedWeights; // Spanning Tree and Loop Edges bool mbFirstConnection; - KeyFrame* mpParent; - std::set mspChildrens; - std::set mspLoopEdges; + std::shared_ptr mpParent; + std::set> mspChildrens; + std::set> mspLoopEdges; // Erase flags bool mbNotErase; diff --git a/orb_slam/include/KeyFrameDatabase.h b/orb_slam/include/KeyFrameDatabase.h index b15c6f43..7cc17d03 100644 --- a/orb_slam/include/KeyFrameDatabase.h +++ b/orb_slam/include/KeyFrameDatabase.h @@ -29,7 +29,7 @@ #include "Frame.h" #include "ORBVocabulary.h" -#include +#include namespace ORB_SLAM @@ -45,17 +45,17 @@ class KeyFrameDatabase KeyFrameDatabase(const ORBVocabulary &voc); - void add(KeyFrame* pKF); + void add(std::shared_ptr pKF); - void erase(KeyFrame* pKF); + void erase(std::shared_ptr pKF); void clear(); // Loop Detection - std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); + std::vector> DetectLoopCandidates(std::shared_ptr pKF, float minScore); // Relocalisation - std::vector DetectRelocalisationCandidates(Frame* F); + std::vector> DetectRelocalisationCandidates(Frame* F); protected: @@ -63,7 +63,7 @@ class KeyFrameDatabase const ORBVocabulary* mpVoc; // Inverted file - std::vector > mvInvertedFile; + std::vector> > mvInvertedFile; // Mutex boost::mutex mMutex; diff --git a/orb_slam/include/LocalMapping.h b/orb_slam/include/LocalMapping.h index 02fa9f0d..f5adf2cf 100644 --- a/orb_slam/include/LocalMapping.h +++ b/orb_slam/include/LocalMapping.h @@ -21,6 +21,8 @@ #ifndef LOCALMAPPING_H #define LOCALMAPPING_H +#include + #include "KeyFrame.h" #include "Map.h" #include "LoopClosing.h" @@ -47,7 +49,7 @@ class LocalMapping void Run(); - void InsertKeyFrame(KeyFrame* pKF); + void InsertKeyFrame(std::shared_ptr pKF); // Thread Synch void RequestStop(); @@ -70,14 +72,14 @@ class LocalMapping bool CheckNewKeyFrames(); void ProcessNewKeyFrame(); - void CreateNewMapPointsMono(); + void CreateNewMapPoints(); void MapPointCulling(); void SearchInNeighbors(); void KeyFrameCulling(); - cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); + cv::Mat ComputeF12(std::shared_ptr &pKF1, std::shared_ptr &pKF2); cv::Mat SkewSymmetricMatrix(const cv::Mat &v); @@ -90,13 +92,13 @@ class LocalMapping LoopClosing* mpLoopCloser; Tracking* mpTracker; - std::list mlNewKeyFrames; + std::list> mlNewKeyFrames; - KeyFrame* mpCurrentKeyFrame; + std::shared_ptr mpCurrentKeyFrame; - std::list mlpRecentAddedMapPoints; + std::list> mlpRecentAddedMapPoints; - boost::mutex mMutexNewKFs; + boost::mutex mMutexNewKFs; bool mbAbortBA; diff --git a/orb_slam/include/LoopClosing.h b/orb_slam/include/LoopClosing.h index da2285ca..1f56f020 100644 --- a/orb_slam/include/LoopClosing.h +++ b/orb_slam/include/LoopClosing.h @@ -44,9 +44,9 @@ class LoopClosing { public: - typedef pair,int> ConsistentGroup; - typedef map, - Eigen::aligned_allocator > > KeyFrameAndPose; + typedef pair>,int> ConsistentGroup; + typedef map,g2o::Sim3,std::less>, + Eigen::aligned_allocator, g2o::Sim3> > > KeyFrameAndPose; public: @@ -58,7 +58,7 @@ class LoopClosing void Run(); - void InsertKeyFrame(KeyFrame *pKF); + void InsertKeyFrame(std::shared_ptrpKF); void RequestReset(); @@ -86,7 +86,7 @@ class LoopClosing LocalMapping *mpLocalMapper; - std::list mlpLoopKeyFrameQueue; + std::list> mlpLoopKeyFrameQueue; boost::mutex mMutexLoopQueue; @@ -96,13 +96,13 @@ class LoopClosing float mnCovisibilityConsistencyTh; // Loop detector variables - KeyFrame* mpCurrentKF; - KeyFrame* mpMatchedKF; + std::shared_ptr mpCurrentKF; + std::shared_ptr mpMatchedKF; std::vector mvConsistentGroups; - std::vector mvpEnoughConsistentCandidates; - std::vector mvpCurrentConnectedKFs; - std::vector mvpCurrentMatchedPoints; - std::vector mvpLoopMapPoints; + std::vector> mvpEnoughConsistentCandidates; + std::vector> mvpCurrentConnectedKFs; + std::vector> mvpCurrentMatchedPoints; + std::vector> mvpLoopMapPoints; cv::Mat mScw; g2o::Sim3 mg2oScw; double mScale_cw; diff --git a/orb_slam/include/Map.h b/orb_slam/include/Map.h index de5a10d8..d7131f06 100644 --- a/orb_slam/include/Map.h +++ b/orb_slam/include/Map.h @@ -23,9 +23,10 @@ #include "MapPoint.h" #include "KeyFrame.h" -#include +#include -#include +#include +#include @@ -40,41 +41,41 @@ class Map public: Map(); - void AddKeyFrame(KeyFrame* pKF); - void AddMapPoint(MapPoint* pMP); - void EraseMapPoint(MapPoint* pMP); - void EraseKeyFrame(KeyFrame* pKF); + void AddKeyFrame(std::shared_ptr pKF); + void AddMapPoint(std::shared_ptr pMP); + void EraseMapPoint(std::shared_ptr pMP); + void EraseKeyFrame(std::shared_ptr pKF); void SetCurrentCameraPose(cv::Mat Tcw); - void SetReferenceKeyFrames(const std::vector &vpKFs); - void SetReferenceMapPoints(const std::vector &vpMPs); + void SetReferenceKeyFrames(const std::vector> &vpKFs); + void SetReferenceMapPoints(const std::vector> &vpMPs); - std::vector GetAllKeyFrames(); - std::vector GetAllMapPoints(); + std::vector> GetAllKeyFrames(); + std::vector> GetAllMapPoints(); cv::Mat GetCameraPose(); - std::vector GetReferenceKeyFrames(); - std::vector GetReferenceMapPoints(); + std::vector> GetReferenceKeyFrames(); + std::vector> GetReferenceMapPoints(); int MapPointsInMap(); int KeyFramesInMap(); void SetFlagAfterBA(); - bool isMapUpdated(); - void ResetUpdated(); + bool isMapUpdated(unsigned int refIdx); + unsigned int GetMapUpdateIdx(); unsigned int GetMaxKFid(); void clear(); protected: - std::set mspMapPoints; - std::set mspKeyFrames; + std::set> mspMapPoints; + std::set> mspKeyFrames; - std::vector mvpReferenceMapPoints; + std::vector> mvpReferenceMapPoints; unsigned int mnMaxKFid; boost::mutex mMutexMap; - bool mbMapUpdated; + unsigned int mbMapUpdateIdx; }; } //namespace ORB_SLAM diff --git a/orb_slam/include/MapPoint.h b/orb_slam/include/MapPoint.h index f743724c..f7bb7c3e 100644 --- a/orb_slam/include/MapPoint.h +++ b/orb_slam/include/MapPoint.h @@ -21,11 +21,13 @@ #ifndef MAPPOINT_H #define MAPPOINT_H -#include -#include "KeyFrame.h" -#include "Map.h" +#include -#include +#include +#include"KeyFrame.h" +#include"Map.h" + +#include namespace ORB_SLAM @@ -36,30 +38,30 @@ class KeyFrame; class Map; -class MapPoint +class MapPoint : public std::enable_shared_from_this { public: - MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap); + MapPoint(const cv::Mat &Pos, std::shared_ptr pRefKF, Map* pMap); void SetWorldPos(const cv::Mat &Pos); cv::Mat GetWorldPos(); cv::Mat GetNormal(); - KeyFrame* GetReferenceKeyFrame(); + std::shared_ptr GetReferenceKeyFrame(); - std::map GetObservations(); + std::map,size_t> GetObservations(); int Observations(); - void AddObservation(KeyFrame* pKF,size_t idx); - void EraseObservation(KeyFrame* pKF); + void AddObservation(std::shared_ptr pKF,size_t idx); + void EraseObservation(std::shared_ptr pKF); - int GetIndexInKeyFrame(KeyFrame* pKF); - bool IsInKeyFrame(KeyFrame* pKF); + int GetIndexInKeyFrame(std::shared_ptr pKF); + bool IsInKeyFrame(std::shared_ptr pKF); void SetBadFlag(); bool isBad(); - void Replace(MapPoint* pMP); + void Replace(std::shared_ptr pMP); void IncreaseVisible(); void IncreaseFound(); @@ -97,13 +99,13 @@ class MapPoint long unsigned int mnCorrectedByKF; long unsigned int mnCorrectedReference; -protected: +protected: // Position in absolute coordinates cv::Mat mWorldPos; // Keyframes observing the point and associated index in keyframe - std::map mObservations; + std::map,size_t> mObservations; // Mean viewing direction cv::Mat mNormalVector; @@ -112,7 +114,7 @@ class MapPoint cv::Mat mDescriptor; // Reference KeyFrame - KeyFrame* mpRefKF; + std::shared_ptr mpRefKF; // Tracking counters int mnVisible; diff --git a/orb_slam/include/MapPublisher.h b/orb_slam/include/MapPublisher.h index 322af246..6224e60d 100644 --- a/orb_slam/include/MapPublisher.h +++ b/orb_slam/include/MapPublisher.h @@ -21,14 +21,14 @@ #ifndef MAPPUBLISHER_H #define MAPPUBLISHER_H -#include +#include + +#include #include -#include -#include "Map.h" -#include "MapPoint.h" -#include "KeyFrame.h" -#include "Converter.h" +#include"Map.h" +#include"MapPoint.h" +#include"KeyFrame.h" namespace ORB_SLAM { @@ -41,11 +41,10 @@ class MapPublisher Map* mpMap; void Refresh(); - void PublishMapPoints(const std::vector &vpMPs, const std::vector &vpRefMPs); - void PublishKeyFrames(const std::vector &vpKFs); + void PublishMapPoints(const std::vector> &vpMPs, const std::vector> &vpRefMPs); + void PublishKeyFrames(const std::vector> &vpKFs); void PublishCurrentCamera(const cv::Mat &Tcw); - void SetCurrentCameraPose(const cv::Mat &Tcw, const double &poseStamp, const float &range, const double &rangeStamp); - float GetRangeScale(); + void SetCurrentCameraPose(const cv::Mat &Tcw); private: @@ -54,7 +53,7 @@ class MapPublisher void ResetCamFlag(); ros::NodeHandle nh; - ros::Publisher mMapPublisher, mPosePublisher; + ros::Publisher publisher; visualization_msgs::Marker mPoints; visualization_msgs::Marker mReferencePoints; @@ -70,12 +69,8 @@ class MapPublisher cv::Mat mCameraPose; bool mbCameraUpdated; - vector< pair > mZposes; - float mLastPose; - float mLastRange; - double mLastStamp; - - float mRangeScale; + //Store the last update counter that have been processed + unsigned int mbLastMapUpdateIdx; boost::mutex mMutexCamera; }; diff --git a/orb_slam/include/ORBVocabulary.h b/orb_slam/include/ORBVocabulary.h index 5746fc37..96a7eddb 100644 --- a/orb_slam/include/ORBVocabulary.h +++ b/orb_slam/include/ORBVocabulary.h @@ -22,8 +22,8 @@ #ifndef ORBVOCABULARY_H #define ORBVOCABULARY_H -#include "Thirdparty/DBoW2/DBoW2/FORB.h" -#include "Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" +#include"Thirdparty/DBoW2/DBoW2/FORB.h" +#include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" namespace ORB_SLAM { diff --git a/orb_slam/include/ORBextractor.h b/orb_slam/include/ORBextractor.h index 3f7c711c..5335f994 100644 --- a/orb_slam/include/ORBextractor.h +++ b/orb_slam/include/ORBextractor.h @@ -32,7 +32,7 @@ namespace ORB_SLAM class ORBextractor { public: - + enum {HARRIS_SCORE=0, FAST_SCORE=1 }; ORBextractor(int nfeatures = 1000, float scaleFactor = 1.2f, int nlevels = 8, int scoreType=FAST_SCORE, int fastTh = 20); diff --git a/orb_slam/include/ORBmatcher.h b/orb_slam/include/ORBmatcher.h index e571ef2d..f479c2b3 100644 --- a/orb_slam/include/ORBmatcher.h +++ b/orb_slam/include/ORBmatcher.h @@ -22,20 +22,22 @@ #ifndef ORBMATCHER_H #define ORBMATCHER_H -#include -#include -#include +#include -#include "MapPoint.h" -#include "KeyFrame.h" -#include "Frame.h" +#include +#include +#include + +#include"MapPoint.h" +#include"KeyFrame.h" +#include"Frame.h" namespace ORB_SLAM { class ORBmatcher -{ +{ public: ORBmatcher(float nnratio=0.6, bool checkOri=true); @@ -45,7 +47,7 @@ class ORBmatcher // Search matches between Frame keypoints and projected MapPoints. Returns number of matches // Used to track the local map (Tracking) - int SearchByProjection(Frame &F, const std::vector &vpMapPoints, const float th=3); + int SearchByProjection(Frame &F, const std::vector> &vpMapPoints, const float th=3); // Project MapPoints tracked in last frame into the current frame and search matches. // Used to track from previous frame (Tracking) @@ -53,39 +55,39 @@ class ORBmatcher // Project MapPoints seen in KeyFrame into the Frame and search matches. // Used in relocalisation (Tracking) - int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set &sAlreadyFound, float th, int ORBdist); + int SearchByProjection(Frame &CurrentFrame, std::shared_ptr pKF, const std::set> &sAlreadyFound, float th, int ORBdist); // Project MapPoints using a Similarity Transformation and search matches. // Used in loop detection (Loop Closing) - int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, std::vector &vpMatched, int th); + int SearchByProjection(std::shared_ptr pKF, cv::Mat Scw, const std::vector> &vpPoints, std::vector> &vpMatched, int th); // Search matches between MapPoints in a KeyFrame and ORB in a Frame. // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) // Used in Relocalisation and Loop Detection - int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); - int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector &vpMatches12); + int SearchByBoW(std::shared_ptrpKF, Frame &F, std::vector> &vpMapPointMatches); + int SearchByBoW(std::shared_ptrpKF1, std::shared_ptr pKF2, std::vector> &vpMatches12); // Search MapPoints tracked in Frame1 in Frame2 in a window centered at their position in Frame1 - int WindowSearch(Frame &F1, Frame &F2, int windowSize, std::vector &vpMapPointMatches2, int minOctave=-1, int maxOctave=INT_MAX); + int WindowSearch(Frame &F1, Frame &F2, int windowSize, std::vector> &vpMapPointMatches2, int minOctave=-1, int maxOctave=INT_MAX); // Refined matching when we have a guess of Frame 2 pose - int SearchByProjection(Frame &F1, Frame &F2, int windowSize, std::vector &vpMapPointMatches2); + int SearchByProjection(Frame &F1, Frame &F2, int windowSize, std::vector> &vpMapPointMatches2); // Matching for the Map Initialization int SearchForInitialization(Frame &F1, Frame &F2, std::vector &vbPrevMatched, std::vector &vnMatches12, int windowSize=10); // Matching to triangulate new MapPoints. Check Epipolar Constraint - int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, + int SearchForTriangulation(std::shared_ptrpKF1, std::shared_ptr pKF2, cv::Mat F12, std::vector &vMatchedKeys1, std::vector &vMatchedKeys2, std::vector > &vMatchedPairs); // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12] - int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, float th); + int SearchBySim3(std::shared_ptr pKF1, std::shared_ptr pKF2, std::vector> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, float th); // Project MapPoints into KeyFrame and search for duplicated MapPoints. - int Fuse(KeyFrame* pKF, std::vector &vpMapPoints, float th=2.5); + int Fuse(std::shared_ptr pKF, std::vector> &vpMapPoints, float th=2.5); // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints. - int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector &vpPoints, float th=2.5); + int Fuse(std::shared_ptr pKF, cv::Mat Scw, const std::vector> &vpPoints, float th=2.5); public: @@ -96,7 +98,7 @@ class ORBmatcher protected: - bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF); + bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const std::shared_ptrpKF); float RadiusByViewingCos(const float &viewCos); @@ -104,7 +106,6 @@ class ORBmatcher float mfNNratio; bool mbCheckOrientation; - }; }// namespace ORB_SLAM diff --git a/orb_slam/include/OctoMapPublisher.h b/orb_slam/include/OctoMapPublisher.h new file mode 100644 index 00000000..ca529747 --- /dev/null +++ b/orb_slam/include/OctoMapPublisher.h @@ -0,0 +1,124 @@ +/** +* This file is part of ORB-SLAM. +* +* Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM. If not, see . +*/ + +#ifndef OCTOMAPPUBLISHER_H +#define OCTOMAPPUBLISHER_H + +#include + +#include + +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +#include "orb_slam/SaveOctomap.h" + +#include"Map.h" +#include"MapPoint.h" +#include"KeyFrame.h" + + +namespace ORB_SLAM +{ + +/** + * This class provides services for providing octomaps and publishes point clouds + */ +class OctoMapPublisher +{ +public: + OctoMapPublisher(Map* pMap); + + void Publish(); + +private: + + bool save(string filename); + + void refreshMap(octomap::OcTree& octoMap); + + void reset(octomap::OcTree& octoMap); + + void mapPointsToOctomap(const std::vector> &vpMPs, octomap::OcTree& m_octoMap); + + void mapPointsToPCL(const std::vector> &vpRefMPs, pcl::PointCloud& pclCloud); + + void octomapToOccupancyGrid(const octomap::OcTree& octree, nav_msgs::OccupancyGrid& map){ + octomapToOccupancyGrid(octree, map, -1.0*std::numeric_limits::max(), std::numeric_limits::max()); + } + + void octomapToOccupancyGrid(const octomap::OcTree& octree, nav_msgs::OccupancyGrid& map, const double minZ_, const double maxZ_ ); + + bool octomapBinarySrv(octomap_msgs::GetOctomapRequest &req, + octomap_msgs::GetOctomapResponse &res); + + bool octomapFullSrv(octomap_msgs::GetOctomapRequest &req, + octomap_msgs::GetOctomapResponse &res); + + bool octomapSaveSrv(orb_slam::SaveOctomapRequest &req, + orb_slam::SaveOctomapResponse &res); + + bool occupancySrv(nav_msgs::GetMapRequest &req, + nav_msgs::GetMapResponse &res); + + void publishPointCloud(); + void publishOctomap(); + void publishProjectedMap(); + + ros::NodeHandle nh; + + double m_projectionMinHeight; + double m_octomapResolution; + + Map* mpMap; + + //Store the last update counter that have been processed + unsigned int mbLastMapUpdateIdx; + + const char* MAP_FRAME_ID; + const char* CAMERA_FRAME_ID; + const char* BASE_LINK_FRAME_ID; + + ros::Publisher publisherPCL; + ros::Publisher publisherOctomapFull; + ros::Publisher publisherOctomapBinary; + ros::Publisher publisherProjected; + + ros::ServiceServer m_octomapSaveService; + ros::ServiceServer m_octomapBinaryService; + ros::ServiceServer m_octomapFullService; + ros::ServiceServer m_occupancyService; + + tf::TransformListener m_tf_listener; +}; + +} //namespace ORB_SLAM + +#endif // OCTOMAPPUBLISHER_H diff --git a/orb_slam/include/Optimizer.h b/orb_slam/include/Optimizer.h index 67ae5126..9fee0c6f 100644 --- a/orb_slam/include/Optimizer.h +++ b/orb_slam/include/Optimizer.h @@ -21,6 +21,8 @@ #ifndef OPTIMIZER_H #define OPTIMIZER_H +#include + #include "Map.h" #include "MapPoint.h" #include "KeyFrame.h" @@ -37,18 +39,18 @@ class LoopClosing; class Optimizer { public: - void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, int nIterations = 5, bool *pbStopFlag=NULL); + void static BundleAdjustment(const std::vector> &vpKF, const std::vector> &vpMP, int nIterations = 5, bool *pbStopFlag=NULL); void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL); - void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag=NULL); + void static LocalBundleAdjustment(std::shared_ptr pKF, bool *pbStopFlag=NULL); int static PoseOptimization(Frame* pFrame); - void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, g2o::Sim3 &Scurw, + void static OptimizeEssentialGraph(Map* pMap, std::shared_ptr pLoopKF, std::shared_ptr pCurKF, g2o::Sim3 &Scurw, LoopClosing::KeyFrameAndPose &NonCorrectedSim3, LoopClosing::KeyFrameAndPose &CorrectedSim3, - std::map > &LoopConnections); + std::map, set> > &LoopConnections); - static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector &vpMatches1, g2o::Sim3 &g2oS12, float th2 = 10); + static int OptimizeSim3(std::shared_ptr pKF1, std::shared_ptr pKF2, std::vector> &vpMatches1, g2o::Sim3 &g2oS12, float th2 = 10); }; } //namespace ORB_SLAM diff --git a/orb_slam/include/PnPsolver.h b/orb_slam/include/PnPsolver.h index 6ed103e5..d769217e 100644 --- a/orb_slam/include/PnPsolver.h +++ b/orb_slam/include/PnPsolver.h @@ -1,6 +1,6 @@ /** * This file is part of ORB-SLAM. -* This is a modified version of EPnP including a RANSAC scheme +* This is a modified version of EPnP , see FreeBSD license below. * * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) * For more information see @@ -19,6 +19,35 @@ * along with ORB-SLAM. If not, see . */ +/** +* Copyright (c) 2009, V. Lepetit, EPFL +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* The views and conclusions contained in the software and documentation are those +* of the authors and should not be interpreted as representing official policies, +* either expressed or implied, of the FreeBSD Project +*/ + #ifndef PNPSOLVER_H #define PNPSOLVER_H @@ -31,7 +60,7 @@ namespace ORB_SLAM class PnPsolver { public: - PnPsolver(const Frame &F, const vector &vpMapPointMatches); + PnPsolver(const Frame &F, const vector> &vpMapPointMatches); ~PnPsolver(); @@ -105,7 +134,7 @@ class PnPsolver { double cws[4][3], ccs[4][3]; double cws_determinant; - vector mvpMapPointMatches; + vector> mvpMapPointMatches; // 2D Points vector mvP2D; diff --git a/orb_slam/include/Sim3Solver.h b/orb_slam/include/Sim3Solver.h index 7e03bc5e..cc1c660a 100644 --- a/orb_slam/include/Sim3Solver.h +++ b/orb_slam/include/Sim3Solver.h @@ -35,7 +35,7 @@ class Sim3Solver { public: - Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12); + Sim3Solver(std::shared_ptr pKF1, std::shared_ptr pKF2, const std::vector> &vpMatched12); void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); @@ -65,14 +65,14 @@ class Sim3Solver protected: // KeyFrames and matches - KeyFrame* mpKF1; - KeyFrame* mpKF2; + std::shared_ptr mpKF1; + std::shared_ptr mpKF2; std::vector mvX3Dc1; std::vector mvX3Dc2; - std::vector mvpMapPoints1; - std::vector mvpMapPoints2; - std::vector mvpMatches12; + std::vector> mvpMapPoints1; + std::vector> mvpMapPoints2; + std::vector> mvpMatches12; std::vector mvnIndices1; std::vector mvSigmaSquare1; std::vector mvSigmaSquare2; diff --git a/orb_slam/include/StatePublisher.h b/orb_slam/include/StatePublisher.h new file mode 100644 index 00000000..f595272b --- /dev/null +++ b/orb_slam/include/StatePublisher.h @@ -0,0 +1,57 @@ +/** +* This file is part of ORB-SLAM. +* +* Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) +* Copyright (C) 2015 Christopher-Eyk Hrabia (Technische Universität Berlin) +* For more information see +* +* ORB-SLAM is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM. If not, see . +*/ + +#ifndef STATEPUBLISHER_H +#define STATEPUBLISHER_H + +#include + +#include"Tracking.h" +#include + +namespace ORB_SLAM +{ + +/** + * This class publishes the current SLAM state + */ +class StatePublisher +{ +public: + StatePublisher(Tracking* tracking); + + void Publish(); + +private: + + ros::NodeHandle nh; + + Tracking* mpTracking; + + ros::Publisher mPublisher; + + orb_slam::ORBState mMsgState; + +}; + +} //namespace ORB_SLAM + +#endif // STATEPUBLISHER_H diff --git a/orb_slam/include/Tracking.h b/orb_slam/include/Tracking.h index 3ae1e736..6726c2e5 100644 --- a/orb_slam/include/Tracking.h +++ b/orb_slam/include/Tracking.h @@ -21,29 +21,24 @@ #ifndef TRACKING_H #define TRACKING_H -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "FramePublisher.h" -#include "Map.h" -#include "LocalMapping.h" -#include "LoopClosing.h" -#include "Frame.h" +#include +#include +#include +#include + +#include"FramePublisher.h" +#include"Map.h" +#include"LocalMapping.h" +#include"LoopClosing.h" +#include"Frame.h" #include "ORBVocabulary.h" -#include "KeyFrameDatabase.h" -#include "ORBextractor.h" +#include"KeyFrameDatabase.h" +#include"ORBextractor.h" #include "Initializer.h" #include "MapPublisher.h" -#include +#include + namespace ORB_SLAM { @@ -54,7 +49,7 @@ class LocalMapping; class LoopClosing; class Tracking -{ +{ public: Tracking(ORBVocabulary* pVoc, FramePublisher* pFramePublisher, MapPublisher* pMapPublisher, Map* pMap, string strSettingPath); @@ -78,7 +73,7 @@ class Tracking void ForceRelocalisation(); eTrackingState mState; - eTrackingState mLastProcessedState; + eTrackingState mLastProcessedState; // Current Frame Frame mCurrentFrame; @@ -90,15 +85,12 @@ class Tracking std::vector mvIniP3D; Frame mInitialFrame; + void CheckResetByPublishers(); protected: - void GrabImages(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info); - - void GrabRange(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::RangeConstPtr& range); + void GrabImage(const sensor_msgs::ImageConstPtr& msg); void FirstInitialization(); void Initialize(); @@ -110,7 +102,7 @@ class Tracking bool TrackWithMotionModel(); bool RelocalisationRequested(); - bool Relocalisation(); + bool Relocalisation(); void UpdateReference(); void UpdateReferencePoints(); @@ -128,23 +120,20 @@ class Tracking LoopClosing* mpLoopClosing; //ORB - ORBextractor* mpORBextractor; - ORBextractor* mpIniORBextractor; + std::shared_ptr mpORBextractor; + std::shared_ptr mpIniORBextractor; //BoW ORBVocabulary* mpORBVocabulary; KeyFrameDatabase* mpKeyFrameDB; // Initalization - Initializer* mpInitializer; - - // False = images are not rectified, True = images rectified - bool mRectified; + std::shared_ptr mpInitializer; //Local Map - KeyFrame* mpReferenceKF; - std::vector mvpLocalKeyFrames; - std::vector mvpLocalMapPoints; + std::shared_ptr mpReferenceKF; + std::vector> mvpLocalKeyFrames; + std::vector> mvpLocalMapPoints; //Publishers FramePublisher* mpFramePublisher; @@ -153,7 +142,7 @@ class Tracking //Map Map* mpMap; - //Camera information + //Calibration matrix cv::Mat mK; cv::Mat mDistCoef; @@ -165,14 +154,14 @@ class Tracking int mnMatchesInliers; //Last Frame, KeyFrame and Relocalisation Info - KeyFrame* mpLastKeyFrame; + std::shared_ptr mpLastKeyFrame; Frame mLastFrame; unsigned int mnLastKeyFrameId; unsigned int mnLastRelocFrameId; //Mutex + boost::mutex mMutexTrack; boost::mutex mMutexForceRelocalisation; - boost::mutex mMutexRange; //Reset bool mbPublisherStopped; @@ -191,25 +180,6 @@ class Tracking // Transfor broadcaster (for visualization in rviz) tf::TransformBroadcaster mTfBr; - - // Range - float mRange; - double mRangeStamp; - - // Image sync - image_transport::SubscriberFilter mImageSub, mImageSub2; - message_filters::Subscriber mInfoSub; - message_filters::Subscriber mRangeSub; - - typedef message_filters::sync_policies::ExactTime policyImages; - typedef message_filters::Synchronizer mPoliceSyncImages; - boost::shared_ptr mSyncImages; - - typedef message_filters::sync_policies::ApproximateTime policyRange; - typedef message_filters::Synchronizer mPoliceSyncRange; - boost::shared_ptr mSyncRange; }; } //namespace ORB_SLAM diff --git a/orb_slam/launch/ExampleFuerte.launch b/orb_slam/launch/ExampleFuerte.launch index 48af8f6d..1fdc6603 100644 --- a/orb_slam/launch/ExampleFuerte.launch +++ b/orb_slam/launch/ExampleFuerte.launch @@ -8,7 +8,7 @@ - + diff --git a/orb_slam/launch/ExampleGroovyHydro.launch b/orb_slam/launch/ExampleGroovyHydro.launch index 4215ecf0..de6c5b85 100644 --- a/orb_slam/launch/ExampleGroovyHydro.launch +++ b/orb_slam/launch/ExampleGroovyHydro.launch @@ -7,8 +7,8 @@ - - + + diff --git a/orb_slam/launch/orb_slam.launch b/orb_slam/launch/orb_slam.launch index 2f1ff853..5db44093 100644 --- a/orb_slam/launch/orb_slam.launch +++ b/orb_slam/launch/orb_slam.launch @@ -7,7 +7,7 @@ - + diff --git a/orb_slam/launch/orb_slam_debug_visualisation.launch b/orb_slam/launch/orb_slam_debug_visualisation.launch new file mode 100644 index 00000000..32b977fc --- /dev/null +++ b/orb_slam/launch/orb_slam_debug_visualisation.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/orb_slam/launch/orb_slam_morse.launch b/orb_slam/launch/orb_slam_morse.launch new file mode 100644 index 00000000..b51ece12 --- /dev/null +++ b/orb_slam/launch/orb_slam_morse.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo.launch b/orb_slam/launch/orb_slam_solo.launch new file mode 100644 index 00000000..62b647c9 --- /dev/null +++ b/orb_slam/launch/orb_slam_solo.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_bluefox_2mm.launch b/orb_slam/launch/orb_slam_solo_bluefox_2mm.launch new file mode 100644 index 00000000..6a560e48 --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_bluefox_2mm.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_bluefox_2mm_stephan.launch b/orb_slam/launch/orb_slam_solo_bluefox_2mm_stephan.launch new file mode 100644 index 00000000..18e7d441 --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_bluefox_2mm_stephan.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_bluefox_6mm.launch b/orb_slam/launch/orb_slam_solo_bluefox_6mm.launch new file mode 100644 index 00000000..d50a10c7 --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_bluefox_6mm.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_hq30_320_240.launch b/orb_slam/launch/orb_slam_solo_hq30_320_240.launch new file mode 100644 index 00000000..4860d83c --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_hq30_320_240.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_logitech_320_240.launch b/orb_slam/launch/orb_slam_solo_logitech_320_240.launch new file mode 100644 index 00000000..0dad7a9b --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_logitech_320_240.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/orb_slam/launch/orb_slam_solo_logitech_640_480.launch b/orb_slam/launch/orb_slam_solo_logitech_640_480.launch new file mode 100644 index 00000000..1f32352b --- /dev/null +++ b/orb_slam/launch/orb_slam_solo_logitech_640_480.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/orb_slam/msg/ORBState.msg b/orb_slam/msg/ORBState.msg new file mode 100644 index 00000000..b4f8007c --- /dev/null +++ b/orb_slam/msg/ORBState.msg @@ -0,0 +1,2 @@ +Header header +uint16 state # State from Tracking.h \ No newline at end of file diff --git a/orb_slam/package.xml b/orb_slam/package.xml index 0a9a1e37..aa8daa03 100644 --- a/orb_slam/package.xml +++ b/orb_slam/package.xml @@ -18,6 +18,11 @@ suitesparse g2o pcl_ros + pcl_conversions + octomap_ros + octomap_msgs + message_generation + std_msgs roscpp tf @@ -27,5 +32,10 @@ cv_bridge g2o pcl_ros + pcl_conversions + octomap_ros + octomap_msgs + message_runtime + std_msgs diff --git a/orb_slam/src/Frame.cc b/orb_slam/src/Frame.cc index e3963b88..0308ce3d 100644 --- a/orb_slam/src/Frame.cc +++ b/orb_slam/src/Frame.cc @@ -20,7 +20,6 @@ #include "Frame.h" #include "Converter.h" -#include "ORBmatcher.h" #include @@ -30,6 +29,7 @@ long unsigned int Frame::nNextId=0; bool Frame::mbInitialComputations=true; float Frame::cx, Frame::cy, Frame::fx, Frame::fy; int Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY; +float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; Frame::Frame() {} @@ -40,7 +40,7 @@ Frame::Frame(const Frame &frame) mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()), N(frame.N), mvKeys(frame.mvKeys), mvKeysUn(frame.mvKeysUn), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec), mDescriptors(frame.mDescriptors.clone()), mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), - mfGridElementWidthInv(frame.mfGridElementWidthInv), mfGridElementHeightInv(frame.mfGridElementHeightInv),mnId(frame.mnId), + mnId(frame.mnId), mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels), mfScaleFactor(frame.mfScaleFactor), mvScaleFactors(frame.mvScaleFactors), mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2) { @@ -53,10 +53,10 @@ Frame::Frame(const Frame &frame) } -Frame::Frame(cv::Mat &im_, const double &timeStamp, ORBextractor* extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef) +Frame::Frame(cv::Mat &im_, const double &timeStamp, std::shared_ptr extractor, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef) :mpORBvocabulary(voc),mpORBextractor(extractor), im(im_),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()) { - // Exctract ORB + // Exctract ORB (*mpORBextractor)(im,cv::Mat(),mvKeys,mDescriptors); N = mvKeys.size(); @@ -64,9 +64,10 @@ Frame::Frame(cv::Mat &im_, const double &timeStamp, ORBextractor* extractor, ORB if(mvKeys.empty()) return; - mvpMapPoints = vector(N,static_cast(NULL)); + mvpMapPoints = vector>(N,static_cast>(NULL)); + + UndistortKeyPoints(); - UndistortKeyPoints(mvKeys, mvKeysUn); // This is done for the first created Frame if(mbInitialComputations) @@ -84,7 +85,8 @@ Frame::Frame(cv::Mat &im_, const double &timeStamp, ORBextractor* extractor, ORB mbInitialComputations=false; } - mnId=nNextId++; + + mnId=nNextId++; //Scale Levels Info mnScaleLevels = mpORBextractor->GetLevels(); @@ -96,7 +98,7 @@ Frame::Frame(cv::Mat &im_, const double &timeStamp, ORBextractor* extractor, ORB mvLevelSigma2[0]=1.0f; for(int i=1; i(N,false); + } void Frame::UpdatePoseMatrices() -{ +{ mRcw = mTcw.rowRange(0,3).colRange(0,3); mtcw = mTcw.rowRange(0,3).col(3); mOw = -mRcw.t()*mtcw; } -bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) +bool Frame::isInFrustum(std::shared_ptr pMP, float viewingCosLimit) { pMP->mbTrackInView = false; // 3D in absolute coordinates - cv::Mat P = pMP->GetWorldPos(); + cv::Mat P = pMP->GetWorldPos(); // 3D in camera coordinates const cv::Mat Pc = mRcw*P+mtcw; @@ -281,20 +286,20 @@ void Frame::ComputeBoW() } } -void Frame::UndistortKeyPoints(const std::vector &kps, std::vector &ukps) +void Frame::UndistortKeyPoints() { if(mDistCoef.at(0)==0.0) { - ukps=kps; + mvKeysUn=mvKeys; return; } // Fill matrix with points - cv::Mat mat(kps.size(),2,CV_32F); - for(unsigned int i=0; i(i,0)=kps[i].pt.x; - mat.at(i,1)=kps[i].pt.y; + mat.at(i,0)=mvKeys[i].pt.x; + mat.at(i,1)=mvKeys[i].pt.y; } // Undistort points @@ -303,13 +308,13 @@ void Frame::UndistortKeyPoints(const std::vector &kps, std::vector mat=mat.reshape(1); // Fill undistorted keypoint vector - ukps.resize(kps.size()); - for(unsigned int i=0; i(i,0); kp.pt.y=mat.at(i,1); - ukps[i]=kp; + mvKeysUn[i]=kp; } } @@ -332,6 +337,7 @@ void Frame::ComputeImageBounds() mnMaxX = max(ceil(mat.at(1,0)),ceil(mat.at(3,0))); mnMinY = min(floor(mat.at(0,1)),floor(mat.at(1,1))); mnMaxY = max(ceil(mat.at(2,1)),ceil(mat.at(3,1))); + } else { diff --git a/orb_slam/src/FramePublisher.cc b/orb_slam/src/FramePublisher.cc index ace8c3eb..fe05229b 100644 --- a/orb_slam/src/FramePublisher.cc +++ b/orb_slam/src/FramePublisher.cc @@ -24,8 +24,8 @@ #include #include -#include -#include +#include +#include #include namespace ORB_SLAM @@ -49,11 +49,15 @@ void FramePublisher::SetMap(Map *pMap) void FramePublisher::Refresh() { + //save computation if their is no subscriber + if(mImagePub.getNumSubscribers() > 0) + { if(mbUpdated) { PublishFrame(); mbUpdated = false; } + } } cv::Mat FramePublisher::DrawFrame() @@ -62,7 +66,7 @@ cv::Mat FramePublisher::DrawFrame() vector vIniKeys; // Initialization: KeyPoints in reference frame vector vMatches; // Initialization: correspondeces with reference keypoints vector vCurrentKeys; // KeyPoints in current frame - vector vMatchedMapPoints; // Tracked MapPoints in current frame + vector> vMatchedMapPoints; // Tracked MapPoints in current frame int state; // Tracking state //Copy variable to be used within scoped mutex @@ -75,7 +79,7 @@ cv::Mat FramePublisher::DrawFrame() mIm.copyTo(im); if(mState==Tracking::NOT_INITIALIZED) - { + { vIniKeys = mvIniKeys; } else if(mState==Tracking::INITIALIZING) @@ -108,7 +112,7 @@ cv::Mat FramePublisher::DrawFrame() cv::line(im,vIniKeys[i].pt,vCurrentKeys[vMatches[i]].pt, cv::Scalar(0,255,0)); } - } + } } else if(state==Tracking::WORKING) //TRACKING { diff --git a/orb_slam/src/Initializer.cc b/orb_slam/src/Initializer.cc index facef939..3ee9cfdf 100644 --- a/orb_slam/src/Initializer.cc +++ b/orb_slam/src/Initializer.cc @@ -25,7 +25,7 @@ #include "Optimizer.h" #include "ORBmatcher.h" -#include +#include namespace ORB_SLAM { @@ -295,13 +295,13 @@ cv::Mat Initializer::ComputeF21(const vector &vP1,const vector(2,2)=0; + w.at(2)=0; return u*cv::Mat::diag(w)*vt; } float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma) -{ +{ const int N = mvMatches12.size(); const float h11 = H21.at(0,0); @@ -479,7 +479,7 @@ bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv: cv::Mat R1, R2, t; // Recover the 4 motion hypotheses - DecomposeE(E21,R1,R2,t); + DecomposeE(E21,R1,R2,t); cv::Mat t1=t; cv::Mat t2=-t; @@ -685,7 +685,7 @@ bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv: int bestGood = 0; - int secondBestGood = 0; + int secondBestGood = 0; int bestSolutionIdx = -1; float bestParallax = -1; vector bestP3D; diff --git a/orb_slam/src/KeyFrame.cc b/orb_slam/src/KeyFrame.cc index ee695c2f..b5bde61f 100644 --- a/orb_slam/src/KeyFrame.cc +++ b/orb_slam/src/KeyFrame.cc @@ -50,7 +50,7 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): mGrid[i][j] = F.mGrid[i][j]; } - SetPose(F.mTcw); + SetPose(F.mTcw); } void KeyFrame::ComputeBoW() @@ -123,7 +123,7 @@ cv::Mat KeyFrame::GetTranslation() return Tcw.rowRange(0,3).col(3).clone(); } -void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) +void KeyFrame::AddConnection(std::shared_ptrpKF, const int &weight) { { boost::mutex::scoped_lock lock(mMutexConnections); @@ -141,13 +141,13 @@ void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) void KeyFrame::UpdateBestCovisibles() { boost::mutex::scoped_lock lock(mMutexConnections); - vector > vPairs; + vector> > vPairs; vPairs.reserve(mConnectedKeyFrameWeights.size()); - for(map::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) + for(map,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) vPairs.push_back(make_pair(mit->second,mit->first)); sort(vPairs.begin(),vPairs.end()); - list lKFs; + list> lKFs; list lWs; for(size_t i=0, iend=vPairs.size(); i(lKFs.begin(),lKFs.end()); - mvOrderedWeights = vector(lWs.begin(), lWs.end()); + mvpOrderedConnectedKeyFrames = vector>(lKFs.begin(),lKFs.end()); + mvOrderedWeights = vector(lWs.begin(), lWs.end()); } -set KeyFrame::GetConnectedKeyFrames() +set> KeyFrame::GetConnectedKeyFrames() { boost::mutex::scoped_lock lock(mMutexConnections); - set s; - for(map::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++) + set> s; + for(map,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++) s.insert(mit->first); return s; } -vector KeyFrame::GetVectorCovisibleKeyFrames() +vector> KeyFrame::GetVectorCovisibleKeyFrames() { boost::mutex::scoped_lock lock(mMutexConnections); return mvpOrderedConnectedKeyFrames; } -vector KeyFrame::GetBestCovisibilityKeyFrames(const int &N) +vector> KeyFrame::GetBestCovisibilityKeyFrames(const int &N) { boost::mutex::scoped_lock lock(mMutexConnections); if((int)mvpOrderedConnectedKeyFrames.size()(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N); + return vector>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N); } -vector KeyFrame::GetCovisiblesByWeight(const int &w) +vector> KeyFrame::GetCovisiblesByWeight(const int &w) { boost::mutex::scoped_lock lock(mMutexConnections); if(mvpOrderedConnectedKeyFrames.empty()) - return vector(); + return vector>(); vector::iterator it = upper_bound(mvOrderedWeights.begin(),mvOrderedWeights.end(),w,KeyFrame::weightComp); if(it==mvOrderedWeights.end()) - return vector(); + return vector>(); else { int n = it-mvOrderedWeights.begin(); - return vector(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n); + return vector>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n); } } -int KeyFrame::GetWeight(KeyFrame *pKF) +int KeyFrame::GetWeight(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexConnections); if(mConnectedKeyFrameWeights.count(pKF)) @@ -210,7 +210,7 @@ int KeyFrame::GetWeight(KeyFrame *pKF) return 0; } -void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx) +void KeyFrame::AddMapPoint(std::shared_ptr pMP, const size_t &idx) { boost::mutex::scoped_lock lock(mMutexFeatures); mvpMapPoints[idx]=pMP; @@ -222,28 +222,28 @@ void KeyFrame::EraseMapPointMatch(const size_t &idx) mvpMapPoints[idx]=NULL; } -void KeyFrame::EraseMapPointMatch(MapPoint* pMP) +void KeyFrame::EraseMapPointMatch(std::shared_ptr pMP) { - int idx = pMP->GetIndexInKeyFrame(this); + int idx = pMP->GetIndexInKeyFrame(shared_from_this()); if(idx>=0) mvpMapPoints[idx]=NULL; } -void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP) +void KeyFrame::ReplaceMapPointMatch(const size_t &idx, std::shared_ptr pMP) { mvpMapPoints[idx]=pMP; } -set KeyFrame::GetMapPoints() +set> KeyFrame::GetMapPoints() { boost::mutex::scoped_lock lock(mMutexFeatures); - set s; + set> s; for(size_t i=0, iend=mvpMapPoints.size(); i pMP = mvpMapPoints[i]; if(!pMP->isBad()) s.insert(pMP); } @@ -264,13 +264,13 @@ int KeyFrame::TrackedMapPoints() return nPoints; } -vector KeyFrame::GetMapPointMatches() +vector> KeyFrame::GetMapPointMatches() { boost::mutex::scoped_lock lock(mMutexFeatures); return mvpMapPoints; } -MapPoint* KeyFrame::GetMapPoint(const size_t &idx) +std::shared_ptr KeyFrame::GetMapPoint(const size_t &idx) { boost::mutex::scoped_lock lock(mMutexFeatures); return mvpMapPoints[idx]; @@ -331,9 +331,9 @@ cv::Mat KeyFrame::GetImage() void KeyFrame::UpdateConnections() { - map KFcounter; + map,int> KFcounter; - vector vpMP; + vector> vpMP; { boost::mutex::scoped_lock lockMPs(mMutexFeatures); @@ -342,9 +342,9 @@ void KeyFrame::UpdateConnections() //For all map points in keyframe check in which other keyframes are they seen //Increase counter for those keyframes - for(vector::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++) + for(vector>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++) { - MapPoint* pMP = *vit; + std::shared_ptr pMP = *vit; if(!pMP) continue; @@ -352,9 +352,9 @@ void KeyFrame::UpdateConnections() if(pMP->isBad()) continue; - map observations = pMP->GetObservations(); + map,size_t> observations = pMP->GetObservations(); - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { if(mit->first->mnId==mnId) continue; @@ -368,12 +368,12 @@ void KeyFrame::UpdateConnections() //If the counter is greater than threshold add connection //In case no keyframe counter is over threshold add the one with maximum counter int nmax=0; - KeyFrame* pKFmax=NULL; + std::shared_ptr pKFmax=NULL; int th = 15; - vector > vPairs; + vector> > vPairs; vPairs.reserve(KFcounter.size()); - for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) + for(map,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { if(mit->second>nmax) { @@ -383,18 +383,18 @@ void KeyFrame::UpdateConnections() if(mit->second>=th) { vPairs.push_back(make_pair(mit->second,mit->first)); - (mit->first)->AddConnection(this,mit->second); + (mit->first)->AddConnection(shared_from_this(),mit->second); } } if(vPairs.empty()) { vPairs.push_back(make_pair(nmax,pKFmax)); - pKFmax->AddConnection(this,nmax); + pKFmax->AddConnection(shared_from_this(),nmax); } sort(vPairs.begin(),vPairs.end()); - list lKFs; + list> lKFs; list lWs; for(size_t i=0; i(lKFs.begin(),lKFs.end()); + mvpOrderedConnectedKeyFrames = vector>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector(lWs.begin(), lWs.end()); if(mbFirstConnection && mnId!=0) { mpParent = mvpOrderedConnectedKeyFrames.front(); - mpParent->AddChild(this); + mpParent->AddChild(shared_from_this()); mbFirstConnection = false; } } } -void KeyFrame::AddChild(KeyFrame *pKF) +void KeyFrame::AddChild(std::shared_ptrpKF) { boost::mutex::scoped_lock lockCon(mMutexConnections); mspChildrens.insert(pKF); } -void KeyFrame::EraseChild(KeyFrame *pKF) +void KeyFrame::EraseChild(std::shared_ptrpKF) { boost::mutex::scoped_lock lockCon(mMutexConnections); mspChildrens.erase(pKF); } -void KeyFrame::ChangeParent(KeyFrame *pKF) +void KeyFrame::ChangeParent(std::shared_ptrpKF) { boost::mutex::scoped_lock lockCon(mMutexConnections); mpParent = pKF; - pKF->AddChild(this); + pKF->AddChild(shared_from_this()); } -set KeyFrame::GetChilds() +set> KeyFrame::GetChilds() { boost::mutex::scoped_lock lockCon(mMutexConnections); return mspChildrens; } -KeyFrame* KeyFrame::GetParent() +std::shared_ptr KeyFrame::GetParent() { boost::mutex::scoped_lock lockCon(mMutexConnections); return mpParent; } -bool KeyFrame::hasChild(KeyFrame *pKF) +bool KeyFrame::hasChild(std::shared_ptrpKF) { boost::mutex::scoped_lock lockCon(mMutexConnections); return mspChildrens.count(pKF); } -void KeyFrame::AddLoopEdge(KeyFrame *pKF) +void KeyFrame::AddLoopEdge(std::shared_ptrpKF) { boost::mutex::scoped_lock lockCon(mMutexConnections); mbNotErase = true; mspLoopEdges.insert(pKF); } -set KeyFrame::GetLoopEdges() +set> KeyFrame::GetLoopEdges() { boost::mutex::scoped_lock lockCon(mMutexConnections); return mspLoopEdges; @@ -495,7 +495,7 @@ void KeyFrame::SetErase() void KeyFrame::SetBadFlag() -{ +{ { boost::mutex::scoped_lock lock(mMutexConnections); if(mnId==0) @@ -507,12 +507,12 @@ void KeyFrame::SetBadFlag() } } - for(map::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) - mit->first->EraseConnection(this); + for(map,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) + mit->first->EraseConnection(shared_from_this()); for(size_t i=0; iEraseObservation(this); + mvpMapPoints[i]->EraseObservation(shared_from_this()); { boost::mutex::scoped_lock lock(mMutexConnections); boost::mutex::scoped_lock lock1(mMutexFeatures); @@ -521,7 +521,7 @@ void KeyFrame::SetBadFlag() mvpOrderedConnectedKeyFrames.clear(); // Update Spanning Tree - set sParentCandidates; + set> sParentCandidates; sParentCandidates.insert(mpParent); // Assign at each iteration one children with a parent (the pair with highest covisibility weight) @@ -531,20 +531,20 @@ void KeyFrame::SetBadFlag() bool bContinue = false; int max = -1; - KeyFrame* pC; - KeyFrame* pP; + std::shared_ptr pC; + std::shared_ptr pP; - for(set::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++) + for(set>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++) { - KeyFrame* pKF = *sit; + std::shared_ptr pKF = *sit; if(pKF->isBad()) continue; // Check if a parent candidate is connected to the keyframe - vector vpConnected = pKF->GetVectorCovisibleKeyFrames(); + vector> vpConnected = pKF->GetVectorCovisibleKeyFrames(); for(size_t i=0, iend=vpConnected.size(); i::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++) + for(set>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++) { if(vpConnected[i]->mnId == (*spcit)->mnId) { @@ -573,18 +573,18 @@ void KeyFrame::SetBadFlag() // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF if(!mspChildrens.empty()) - for(set::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++) + for(set>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++) { (*sit)->ChangeParent(mpParent); } - mpParent->EraseChild(this); + mpParent->EraseChild(shared_from_this()); mbBad = true; } - mpMap->EraseKeyFrame(this); - mpKeyFrameDB->erase(this); + mpMap->EraseKeyFrame(shared_from_this()); + mpKeyFrameDB->erase(shared_from_this()); } bool KeyFrame::isBad() @@ -593,7 +593,7 @@ bool KeyFrame::isBad() return mbBad; } -void KeyFrame::EraseConnection(KeyFrame* pKF) +void KeyFrame::EraseConnection(std::shared_ptr pKF) { bool bUpdate = false; { @@ -658,7 +658,7 @@ bool KeyFrame::IsInImage(const float &x, const float &y) const float KeyFrame::ComputeSceneMedianDepth(int q) { - vector vpMapPoints; + vector> vpMapPoints; cv::Mat Tcw_; { boost::mutex::scoped_lock lock(mMutexFeatures); @@ -676,7 +676,7 @@ float KeyFrame::ComputeSceneMedianDepth(int q) { if(mvpMapPoints[i]) { - MapPoint* pMP = mvpMapPoints[i]; + std::shared_ptr pMP = mvpMapPoints[i]; cv::Mat x3Dw = pMP->GetWorldPos(); float z = Rcw2.dot(x3Dw)+zcw; vDepths.push_back(z); diff --git a/orb_slam/src/KeyFrameDatabase.cc b/orb_slam/src/KeyFrameDatabase.cc index 968670d4..f253a21d 100644 --- a/orb_slam/src/KeyFrameDatabase.cc +++ b/orb_slam/src/KeyFrameDatabase.cc @@ -36,7 +36,7 @@ KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc): } -void KeyFrameDatabase::add(KeyFrame *pKF) +void KeyFrameDatabase::add(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutex); @@ -44,7 +44,7 @@ void KeyFrameDatabase::add(KeyFrame *pKF) mvInvertedFile[vit->first].push_back(pKF); } -void KeyFrameDatabase::erase(KeyFrame* pKF) +void KeyFrameDatabase::erase(std::shared_ptr pKF) { boost::mutex::scoped_lock lock(mMutex); @@ -52,9 +52,9 @@ void KeyFrameDatabase::erase(KeyFrame* pKF) for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++) { // List of keyframes that share the word - list &lKFs = mvInvertedFile[vit->first]; + list> &lKFs = mvInvertedFile[vit->first]; - for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { if(pKF==*lit) { @@ -72,10 +72,10 @@ void KeyFrameDatabase::clear() } -vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore) +vector> KeyFrameDatabase::DetectLoopCandidates(std::shared_ptr pKF, float minScore) { - set spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); - list lKFsSharingWords; + set> spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); + list> lKFsSharingWords; // Search all keyframes that share a word with current keyframes // Discard keyframes connected to the query keyframe @@ -84,11 +84,11 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++) { - list &lKFs = mvInvertedFile[vit->first]; + list> &lKFs = mvInvertedFile[vit->first]; - for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { - KeyFrame* pKFi=*lit; + std::shared_ptr pKFi=*lit; if(pKFi->mnLoopQuery!=pKF->mnId) { pKFi->mnLoopWords=0; @@ -104,13 +104,13 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi } if(lKFsSharingWords.empty()) - return vector(); + return vector>(); - list > lScoreAndMatch; + list> > lScoreAndMatch; // Only compare against those keyframes that share enough words int maxCommonWords=0; - for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { if((*lit)->mnLoopWords>maxCommonWords) maxCommonWords=(*lit)->mnLoopWords; @@ -121,9 +121,9 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi int nscores=0; // Compute similarity score. Retain the matches whose score is higher than minScore - for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { - KeyFrame* pKFi = *lit; + std::shared_ptr pKFi = *lit; if(pKFi->mnLoopWords>minCommonWords) { @@ -138,23 +138,23 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi } if(lScoreAndMatch.empty()) - return vector(); + return vector>(); - list > lAccScoreAndMatch; + list> > lAccScoreAndMatch; float bestAccScore = minScore; // Lets now accumulate score by covisibility - for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) + for(list> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) { - KeyFrame* pKFi = it->second; - vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + std::shared_ptr pKFi = it->second; + vector> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); float bestScore = it->first; float accScore = it->first; - KeyFrame* pBestKF = pKFi; - for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) + std::shared_ptr pBestKF = pKFi; + for(vector>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) { - KeyFrame* pKF2 = *vit; + std::shared_ptr pKF2 = *vit; if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords) { accScore+=pKF2->mLoopScore; @@ -174,15 +174,15 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain = 0.75f*bestAccScore; - set spAlreadyAddedKF; - vector vpLoopCandidates; + set> spAlreadyAddedKF; + vector> vpLoopCandidates; vpLoopCandidates.reserve(lAccScoreAndMatch.size()); - for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) + for(list> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) { if(it->first>minScoreToRetain) { - KeyFrame* pKFi = it->second; + std::shared_ptr pKFi = it->second; if(!spAlreadyAddedKF.count(pKFi)) { vpLoopCandidates.push_back(pKFi); @@ -195,9 +195,9 @@ vector KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float mi return vpLoopCandidates; } -vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) +vector> KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) { - list lKFsSharingWords; + list> lKFsSharingWords; // Search all keyframes that share a word with current frame { @@ -205,11 +205,11 @@ vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++) { - list &lKFs = mvInvertedFile[vit->first]; + list> &lKFs = mvInvertedFile[vit->first]; - for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { - KeyFrame* pKFi=*lit; + std::shared_ptr pKFi=*lit; if(pKFi->mnRelocQuery!=F->mnId) { pKFi->mnRelocWords=0; @@ -221,11 +221,11 @@ vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) } } if(lKFsSharingWords.empty()) - return vector(); + return vector>(); // Only compare against those keyframes that share enough words int maxCommonWords=0; - for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { if((*lit)->mnRelocWords>maxCommonWords) maxCommonWords=(*lit)->mnRelocWords; @@ -233,14 +233,14 @@ vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) int minCommonWords = maxCommonWords*0.8f; - list > lScoreAndMatch; + list> > lScoreAndMatch; int nscores=0; // Compute similarity score. - for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) + for(list>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { - KeyFrame* pKFi = *lit; + std::shared_ptr pKFi = *lit; if(pKFi->mnRelocWords>minCommonWords) { @@ -252,23 +252,23 @@ vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) } if(lScoreAndMatch.empty()) - return vector(); + return vector>(); - list > lAccScoreAndMatch; + list> > lAccScoreAndMatch; float bestAccScore = 0; // Lets now accumulate score by covisibility - for(list >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) + for(list> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) { - KeyFrame* pKFi = it->second; - vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); + std::shared_ptr pKFi = it->second; + vector> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); float bestScore = it->first; float accScore = bestScore; - KeyFrame* pBestKF = pKFi; - for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) + std::shared_ptr pBestKF = pKFi; + for(vector>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) { - KeyFrame* pKF2 = *vit; + std::shared_ptr pKF2 = *vit; if(pKF2->mnRelocQuery!=F->mnId) continue; @@ -287,15 +287,15 @@ vector KeyFrameDatabase::DetectRelocalisationCandidates(Frame *F) // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain = 0.75f*bestAccScore; - set spAlreadyAddedKF; - vector vpRelocCandidates; + set> spAlreadyAddedKF; + vector> vpRelocCandidates; vpRelocCandidates.reserve(lAccScoreAndMatch.size()); - for(list >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) + for(list> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) { const float &si = it->first; if(si>minScoreToRetain) { - KeyFrame* pKFi = it->second; + std::shared_ptr pKFi = it->second; if(!spAlreadyAddedKF.count(pKFi)) { vpRelocCandidates.push_back(pKFi); diff --git a/orb_slam/src/LocalMapping.cc b/orb_slam/src/LocalMapping.cc index f16d1c72..2f923cc8 100644 --- a/orb_slam/src/LocalMapping.cc +++ b/orb_slam/src/LocalMapping.cc @@ -29,7 +29,7 @@ namespace ORB_SLAM { LocalMapping::LocalMapping(Map *pMap): - mbResetRequested(false), mpMap(pMap), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbAcceptKeyFrames(true) + mbResetRequested(false), mpMap(pMap), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbAcceptKeyFrames(true) { } @@ -51,7 +51,7 @@ void LocalMapping::Run() { // Check if there are keyframes in the queue if(CheckNewKeyFrames()) - { + { // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(false); @@ -62,7 +62,7 @@ void LocalMapping::Run() MapPointCulling(); // Triangulate new MapPoints - CreateNewMapPointsMono(); + CreateNewMapPoints(); // Find more matches in neighbor keyframes and fuse point duplications SearchInNeighbors(); @@ -105,7 +105,7 @@ void LocalMapping::Run() } } -void LocalMapping::InsertKeyFrame(KeyFrame *pKF) +void LocalMapping::InsertKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexNewKFs); mlNewKeyFrames.push_back(pKF); @@ -135,12 +135,12 @@ void LocalMapping::ProcessNewKeyFrame() return; // Associate MapPoints to the new keyframe and update normal and descriptor - vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + vector> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); if(mpCurrentKeyFrame->mnId>1) //This operations are already done in the tracking for the first two keyframes { for(size_t i=0; i pMP = vpMapPointMatches[i]; if(pMP) { if(!pMP->isBad()) @@ -157,13 +157,13 @@ void LocalMapping::ProcessNewKeyFrame() { for(size_t i=0; i pMP = vpMapPointMatches[i]; if(pMP) { mlpRecentAddedMapPoints.push_back(pMP); } } - } + } // Update links in the Covisibility Graph mpCurrentKeyFrame->UpdateConnections(); @@ -175,11 +175,11 @@ void LocalMapping::ProcessNewKeyFrame() void LocalMapping::MapPointCulling() { // Check Recent Added MapPoints - list::iterator lit = mlpRecentAddedMapPoints.begin(); + list>::iterator lit = mlpRecentAddedMapPoints.begin(); const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId; while(lit!=mlpRecentAddedMapPoints.end()) { - MapPoint* pMP = *lit; + std::shared_ptr pMP = *lit; if(pMP->isBad()) { lit = mlpRecentAddedMapPoints.erase(lit); @@ -202,10 +202,10 @@ void LocalMapping::MapPointCulling() } } -void LocalMapping::CreateNewMapPointsMono() +void LocalMapping::CreateNewMapPoints() { // Take neighbor keyframes in covisibility graph - vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20); + vector> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20); ORBmatcher matcher(0.6,false); @@ -229,7 +229,7 @@ void LocalMapping::CreateNewMapPointsMono() // Search matches with epipolar restriction and triangulate for(size_t i=0; i pKF2 = vpNeighKFs[i]; // Check first that baseline is not too short // Small translation errors for short baseline keyframes make scale to diverge @@ -352,7 +352,7 @@ void LocalMapping::CreateNewMapPointsMono() continue; // Triangulation is succesfull - MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap); + std::shared_ptr pMP(new MapPoint(x3D,mpCurrentKeyFrame,mpMap)); pMP->AddObservation(pKF2,idx2); pMP->AddObservation(mpCurrentKeyFrame,idx1); @@ -373,21 +373,21 @@ void LocalMapping::CreateNewMapPointsMono() void LocalMapping::SearchInNeighbors() { // Retrieve neighbor keyframes - vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20); - vector vpTargetKFs; - for(vector::iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++) + vector> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20); + vector> vpTargetKFs; + for(vector>::iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++) { - KeyFrame* pKFi = *vit; + std::shared_ptr pKFi = *vit; if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi); pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId; // Extend to some second neighbors - vector vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5); - for(vector::iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++) + vector> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5); + for(vector>::iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++) { - KeyFrame* pKFi2 = *vit2; + std::shared_ptr pKFi2 = *vit2; if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi2); @@ -397,27 +397,27 @@ void LocalMapping::SearchInNeighbors() // Search matches by projection from current KF in target KFs ORBmatcher matcher(0.6); - vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); - for(vector::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++) + vector> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); + for(vector>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++) { - KeyFrame* pKFi = *vit; + std::shared_ptr pKFi = *vit; matcher.Fuse(pKFi,vpMapPointMatches); } // Search matches by projection from target KFs in current KF - vector vpFuseCandidates; + vector> vpFuseCandidates; vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size()); - for(vector::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++) + for(vector>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++) { - KeyFrame* pKFi = *vitKF; + std::shared_ptr pKFi = *vitKF; - vector vpMapPointsKFi = pKFi->GetMapPointMatches(); + vector> vpMapPointsKFi = pKFi->GetMapPointMatches(); - for(vector::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++) + for(vector>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++) { - MapPoint* pMP = *vitMP; + std::shared_ptr pMP = *vitMP; if(!pMP) continue; if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId) @@ -434,7 +434,7 @@ void LocalMapping::SearchInNeighbors() vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); for(size_t i=0, iend=vpMapPointMatches.size(); i pMP=vpMapPointMatches[i]; if(pMP) { if(!pMP->isBad()) @@ -449,7 +449,7 @@ void LocalMapping::SearchInNeighbors() mpCurrentKeyFrame->UpdateConnections(); } -cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) +cv::Mat LocalMapping::ComputeF12(std::shared_ptr&pKF1, std::shared_ptr&pKF2) { cv::Mat R1w = pKF1->GetRotation(); cv::Mat t1w = pKF1->GetTranslation(); @@ -499,8 +499,6 @@ void LocalMapping::Release() boost::mutex::scoped_lock lock(mMutexStop); mbStopped = false; mbStopRequested = false; - for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) - delete *lit; mlNewKeyFrames.clear(); } @@ -526,20 +524,20 @@ void LocalMapping::KeyFrameCulling() // Check redundant keyframes (only local keyframes) // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen // in at least other 3 keyframes (in the same or finer scale) - vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); + vector> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); - for(vector::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++) + for(vector>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++) { - KeyFrame* pKF = *vit; + std::shared_ptr pKF = *vit; if(pKF->mnId==0) continue; - vector vpMapPoints = pKF->GetMapPointMatches(); + vector> vpMapPoints = pKF->GetMapPointMatches(); int nRedundantObservations=0; int nMPs=0; for(size_t i=0, iend=vpMapPoints.size(); i pMP = vpMapPoints[i]; if(pMP) { if(!pMP->isBad()) @@ -548,11 +546,11 @@ void LocalMapping::KeyFrameCulling() if(pMP->Observations()>3) { int scaleLevel = pKF->GetKeyPointUn(i).octave; - map observations = pMP->GetObservations(); + map, size_t> observations = pMP->GetObservations(); int nObs=0; - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map, size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKFi = mit->first; + std::shared_ptr pKFi = mit->first; if(pKFi==pKF) continue; int scaleLeveli = pKFi->GetKeyPointUn(mit->second).octave; diff --git a/orb_slam/src/LoopClosing.cc b/orb_slam/src/LoopClosing.cc index 25251ce3..530fbc42 100644 --- a/orb_slam/src/LoopClosing.cc +++ b/orb_slam/src/LoopClosing.cc @@ -74,7 +74,7 @@ void LoopClosing::Run() } } -void LoopClosing::InsertKeyFrame(KeyFrame *pKF) +void LoopClosing::InsertKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexLoopQueue); if(pKF->mnId!=0) @@ -108,12 +108,12 @@ bool LoopClosing::DetectLoop() // Compute reference BoW similarity score // This is the lowest score to a connected keyframe in the covisibility graph // We will impose loop candidates to have a higher similarity than this - vector vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames(); + vector> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames(); DBoW2::BowVector CurrentBowVec = mpCurrentKF->GetBowVector(); float minScore = 1; for(size_t i=0; i pKF = vpConnectedKeyFrames[i]; if(pKF->isBad()) continue; DBoW2::BowVector BowVec = pKF->GetBowVector(); @@ -125,7 +125,7 @@ bool LoopClosing::DetectLoop() } // Query the database imposing the minimum score - vector vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore); + vector> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore); // If there are no loop candidates, just add new keyframe and return false @@ -147,19 +147,19 @@ bool LoopClosing::DetectLoop() vector vbConsistentGroup(mvConsistentGroups.size(),false); for(size_t i=0, iend=vpCandidateKFs.size(); i pCandidateKF = vpCandidateKFs[i]; - set spCandidateGroup = pCandidateKF->GetConnectedKeyFrames(); + set> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames(); spCandidateGroup.insert(pCandidateKF); bool bEnoughConsistent = false; bool bConsistentForSomeGroup = false; for(size_t iG=0, iendG=mvConsistentGroups.size(); iG sPreviousGroup = mvConsistentGroups[iG].first; + set> sPreviousGroup = mvConsistentGroups[iG].first; bool bConsistent = false; - for(set::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++) + for(set>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++) { if(sPreviousGroup.count(*sit)) { @@ -226,10 +226,10 @@ bool LoopClosing::ComputeSim3() // If enough matches are found, we setup a Sim3Solver ORBmatcher matcher(0.75,true); - vector vpSim3Solvers; + vector> vpSim3Solvers; vpSim3Solvers.resize(nInitialCandidates); - vector > vvpMapPointMatches; + vector> > vvpMapPointMatches; vvpMapPointMatches.resize(nInitialCandidates); vector vbDiscarded; @@ -239,7 +239,7 @@ bool LoopClosing::ComputeSim3() for(int i=0; i pKF = mvpEnoughConsistentCandidates[i]; // avoid that local mapping erase it while it is being processed in this thread pKF->SetNotErase(); @@ -259,7 +259,7 @@ bool LoopClosing::ComputeSim3() } else { - Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i]); + shared_ptr pSolver(new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i])); pSolver->SetRansacParameters(0.99,20,300); vpSim3Solvers[i] = pSolver; } @@ -278,14 +278,14 @@ bool LoopClosing::ComputeSim3() if(vbDiscarded[i]) continue; - KeyFrame* pKF = mvpEnoughConsistentCandidates[i]; + std::shared_ptr pKF = mvpEnoughConsistentCandidates[i]; // Perform 5 Ransac Iterations vector vbInliers; int nInliers; bool bNoMore; - Sim3Solver* pSolver = vpSim3Solvers[i]; + shared_ptr pSolver = vpSim3Solvers[i]; cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers); // If Ransac reachs max. iterations discard keyframe @@ -298,7 +298,7 @@ bool LoopClosing::ComputeSim3() // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences if(!Scm.empty()) { - vector vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast(NULL)); + vector> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast>(NULL)); for(size_t j=0, jend=vbInliers.size(); j vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames(); + vector> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames(); vpLoopConnectedKFs.push_back(mpMatchedKF); mvpLoopMapPoints.clear(); - for(vector::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++) + for(vector>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++) { - KeyFrame* pKF = *vit; - vector vpMapPoints = pKF->GetMapPointMatches(); + std::shared_ptr pKF = *vit; + vector> vpMapPoints = pKF->GetMapPointMatches(); for(size_t i=0, iend=vpMapPoints.size(); i pMP = vpMapPoints[i]; if(pMP) { if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId) @@ -413,9 +413,9 @@ void LoopClosing::CorrectLoop() cv::Mat Twc = mpCurrentKF->GetPoseInverse(); - for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + for(vector>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) { - KeyFrame* pKFi = *vit; + std::shared_ptr pKFi = *vit; cv::Mat Tiw = pKFi->GetPose(); @@ -440,16 +440,16 @@ void LoopClosing::CorrectLoop() // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++) { - KeyFrame* pKFi = mit->first; + std::shared_ptr pKFi = mit->first; g2o::Sim3 g2oCorrectedSiw = mit->second; g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse(); g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi]; - vector vpMPsi = pKFi->GetMapPointMatches(); + vector> vpMPsi = pKFi->GetMapPointMatches(); for(size_t iMP=0, endMPi = vpMPsi.size(); iMP pMPi = vpMPsi[iMP]; if(!pMPi) continue; if(pMPi->isBad()) @@ -490,8 +490,8 @@ void LoopClosing::CorrectLoop() { if(mvpCurrentMatchedPoints[i]) { - MapPoint* pLoopMP = mvpCurrentMatchedPoints[i]; - MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); + std::shared_ptr pLoopMP = mvpCurrentMatchedPoints[i]; + std::shared_ptr pCurMP = mpCurrentKF->GetMapPoint(i); if(pCurMP) pCurMP->Replace(pLoopMP); else @@ -510,21 +510,21 @@ void LoopClosing::CorrectLoop() // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop - map > LoopConnections; + map, set> > LoopConnections; - for(vector::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) + for(vector>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) { - KeyFrame* pKFi = *vit; - vector vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); + std::shared_ptr pKFi = *vit; + vector> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); // Update connections. Detect new links. pKFi->UpdateConnections(); LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames(); - for(vector::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++) + for(vector>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++) { LoopConnections[pKFi].erase(*vit_prev); } - for(vector::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++) + for(vector>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++) { LoopConnections[pKFi].erase(*vit2); } @@ -554,7 +554,7 @@ void LoopClosing::SearchAndFuse(KeyFrameAndPose &CorrectedPosesMap) for(KeyFrameAndPose::iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; g2o::Sim3 g2oScw = mit->second; cv::Mat cvScw = Converter::toCvMat(g2oScw); diff --git a/orb_slam/src/Map.cc b/orb_slam/src/Map.cc index 371ecbc5..dff44c99 100644 --- a/orb_slam/src/Map.cc +++ b/orb_slam/src/Map.cc @@ -18,11 +18,6 @@ * along with ORB-SLAM. If not, see . */ -#include -#include - -#include - #include "Map.h" namespace ORB_SLAM @@ -30,57 +25,57 @@ namespace ORB_SLAM Map::Map() { - mbMapUpdated= false; + mbMapUpdateIdx= false; mnMaxKFid = 0; } -void Map::AddKeyFrame(KeyFrame *pKF) +void Map::AddKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexMap); mspKeyFrames.insert(pKF); if(pKF->mnId>mnMaxKFid) mnMaxKFid=pKF->mnId; - mbMapUpdated=true; + mbMapUpdateIdx++; } -void Map::AddMapPoint(MapPoint *pMP) +void Map::AddMapPoint(std::shared_ptr pMP) { boost::mutex::scoped_lock lock(mMutexMap); mspMapPoints.insert(pMP); - mbMapUpdated=true; + mbMapUpdateIdx++; } -void Map::EraseMapPoint(MapPoint *pMP) +void Map::EraseMapPoint(std::shared_ptr pMP) { boost::mutex::scoped_lock lock(mMutexMap); mspMapPoints.erase(pMP); - mbMapUpdated=true; + mbMapUpdateIdx++; } -void Map::EraseKeyFrame(KeyFrame *pKF) +void Map::EraseKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexMap); mspKeyFrames.erase(pKF); - mbMapUpdated=true; + mbMapUpdateIdx++; } -void Map::SetReferenceMapPoints(const vector &vpMPs) +void Map::SetReferenceMapPoints(const vector> &vpMPs) { boost::mutex::scoped_lock lock(mMutexMap); mvpReferenceMapPoints = vpMPs; - mbMapUpdated=true; + mbMapUpdateIdx++; } -vector Map::GetAllKeyFrames() +vector> Map::GetAllKeyFrames() { boost::mutex::scoped_lock lock(mMutexMap); - return vector(mspKeyFrames.begin(),mspKeyFrames.end()); + return vector>(mspKeyFrames.begin(),mspKeyFrames.end()); } -vector Map::GetAllMapPoints() +vector> Map::GetAllMapPoints() { boost::mutex::scoped_lock lock(mMutexMap); - return vector(mspMapPoints.begin(),mspMapPoints.end()); + return vector>(mspMapPoints.begin(),mspMapPoints.end()); } int Map::MapPointsInMap() @@ -95,29 +90,27 @@ int Map::KeyFramesInMap() return mspKeyFrames.size(); } -vector Map::GetReferenceMapPoints() +vector> Map::GetReferenceMapPoints() { boost::mutex::scoped_lock lock(mMutexMap); - return mvpReferenceMapPoints; + return vector>(mvpReferenceMapPoints.begin(),mvpReferenceMapPoints.end()); } -bool Map::isMapUpdated() +unsigned int Map::GetMapUpdateIdx() { - boost::mutex::scoped_lock lock(mMutexMap); - return mbMapUpdated; + return mbMapUpdateIdx; } -void Map::SetFlagAfterBA() +bool Map::isMapUpdated(unsigned int refIdx) { - boost::mutex::scoped_lock lock(mMutexMap); - mbMapUpdated=true; - + return mbMapUpdateIdx != refIdx; } -void Map::ResetUpdated() +void Map::SetFlagAfterBA() { boost::mutex::scoped_lock lock(mMutexMap); - mbMapUpdated=false; + mbMapUpdateIdx++; + } unsigned int Map::GetMaxKFid() @@ -128,12 +121,6 @@ unsigned int Map::GetMaxKFid() void Map::clear() { - for(set::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++) - delete *sit; - - for(set::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++) - delete *sit; - mspMapPoints.clear(); mspKeyFrames.clear(); mnMaxKFid = 0; diff --git a/orb_slam/src/MapPoint.cc b/orb_slam/src/MapPoint.cc index 63160c86..e16c5943 100644 --- a/orb_slam/src/MapPoint.cc +++ b/orb_slam/src/MapPoint.cc @@ -28,7 +28,7 @@ namespace ORB_SLAM long unsigned int MapPoint::nNextId=0; -MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap): +MapPoint::MapPoint(const cv::Mat &Pos, std::shared_ptrpRefKF, Map* pMap): mnFirstKFid(pRefKF->mnId), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),mnCorrectedReference(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap) @@ -56,19 +56,19 @@ cv::Mat MapPoint::GetNormal() return mNormalVector.clone(); } -KeyFrame* MapPoint::GetReferenceKeyFrame() +std::shared_ptr MapPoint::GetReferenceKeyFrame() { boost::mutex::scoped_lock lock(mMutexFeatures); return mpRefKF; } -void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) +void MapPoint::AddObservation(std::shared_ptr pKF, size_t idx) { boost::mutex::scoped_lock lock(mMutexFeatures); mObservations[pKF]=idx; } -void MapPoint::EraseObservation(KeyFrame* pKF) +void MapPoint::EraseObservation(std::shared_ptr pKF) { bool bBad=false; { @@ -90,7 +90,7 @@ void MapPoint::EraseObservation(KeyFrame* pKF) SetBadFlag(); } -map MapPoint::GetObservations() +map, size_t> MapPoint::GetObservations() { boost::mutex::scoped_lock lock(mMutexFeatures); return mObservations; @@ -104,7 +104,7 @@ int MapPoint::Observations() void MapPoint::SetBadFlag() { - map obs; + map,size_t> obs; { boost::mutex::scoped_lock lock1(mMutexFeatures); boost::mutex::scoped_lock lock2(mMutexPos); @@ -112,21 +112,21 @@ void MapPoint::SetBadFlag() obs = mObservations; mObservations.clear(); } - for(map::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; pKF->EraseMapPointMatch(mit->second); } - mpMap->EraseMapPoint(this); + mpMap->EraseMapPoint(shared_from_this()); } -void MapPoint::Replace(MapPoint* pMP) +void MapPoint::Replace(std::shared_ptr pMP) { if(pMP->mnId==this->mnId) return; - map obs; + map,size_t> obs; { boost::mutex::scoped_lock lock1(mMutexFeatures); boost::mutex::scoped_lock lock2(mMutexPos); @@ -135,10 +135,10 @@ void MapPoint::Replace(MapPoint* pMP) mbBad=true; } - for(map::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++) { // Replace measurement in keyframe - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; if(!pMP->IsInKeyFrame(pKF)) { @@ -153,7 +153,7 @@ void MapPoint::Replace(MapPoint* pMP) pMP->ComputeDistinctiveDescriptors(); - mpMap->EraseMapPoint(this); + mpMap->EraseMapPoint(shared_from_this()); } @@ -187,7 +187,7 @@ void MapPoint::ComputeDistinctiveDescriptors() // Retrieve all observed descriptors vector vDescriptors; - map observations; + map,size_t> observations; { boost::mutex::scoped_lock lock1(mMutexFeatures); @@ -201,9 +201,9 @@ void MapPoint::ComputeDistinctiveDescriptors() vDescriptors.reserve(observations.size()); - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; if(!pKF->isBad()) vDescriptors.push_back(pKF->GetDescriptor(mit->second)); @@ -245,7 +245,7 @@ void MapPoint::ComputeDistinctiveDescriptors() { boost::mutex::scoped_lock lock(mMutexFeatures); - mDescriptor = vDescriptors[BestIdx].clone(); + mDescriptor = vDescriptors[BestIdx].clone(); } } @@ -255,7 +255,7 @@ cv::Mat MapPoint::GetDescriptor() return mDescriptor.clone(); } -int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) +int MapPoint::GetIndexInKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexFeatures); if(mObservations.count(pKF)) @@ -264,7 +264,7 @@ int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) return -1; } -bool MapPoint::IsInKeyFrame(KeyFrame *pKF) +bool MapPoint::IsInKeyFrame(std::shared_ptrpKF) { boost::mutex::scoped_lock lock(mMutexFeatures); return (mObservations.count(pKF)); @@ -272,35 +272,35 @@ bool MapPoint::IsInKeyFrame(KeyFrame *pKF) void MapPoint::UpdateNormalAndDepth() { - map observations; - KeyFrame* pRefKF; + map,size_t> observations; + std::shared_ptr pRefKF; cv::Mat Pos; { boost::mutex::scoped_lock lock1(mMutexFeatures); boost::mutex::scoped_lock lock2(mMutexPos); if(mbBad) return; - observations = mObservations; - pRefKF = mpRefKF; + observations=mObservations; + pRefKF=mpRefKF; Pos = mWorldPos.clone(); } cv::Mat normal = cv::Mat::zeros(3,1,CV_32F); int n=0; - for(map::iterator mit = observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; cv::Mat Owi = pKF->GetCameraCenter(); cv::Mat normali = mWorldPos - Owi; normal = normal + normali/cv::norm(normali); n++; - } + } cv::Mat PC = Pos - pRefKF->GetCameraCenter(); const float dist = cv::norm(PC); const int level = pRefKF->GetKeyPointScaleLevel(observations[pRefKF]); const float scaleFactor = pRefKF->GetScaleFactor(); - const float levelScaleFactor = pRefKF->GetScaleFactor(level); + const float levelScaleFactor = pRefKF->GetScaleFactor(level); const int nLevels = pRefKF->GetScaleLevels(); { diff --git a/orb_slam/src/MapPublisher.cc b/orb_slam/src/MapPublisher.cc index 57db7877..e3b26a50 100644 --- a/orb_slam/src/MapPublisher.cc +++ b/orb_slam/src/MapPublisher.cc @@ -26,7 +26,7 @@ namespace ORB_SLAM { -MapPublisher::MapPublisher(Map* pMap):mpMap(pMap), mbCameraUpdated(false), mLastPose(-1.0), mLastRange(-1.0), mLastStamp(-1.0), mRangeScale(-1.0) +MapPublisher::MapPublisher(Map* pMap):mpMap(pMap), mbCameraUpdated(false), mbLastMapUpdateIdx(0) { const char* MAP_FRAME_ID = "/ORB_SLAM/World"; const char* POINTS_NAMESPACE = "MapPoints"; @@ -106,44 +106,49 @@ MapPublisher::MapPublisher(Map* pMap):mpMap(pMap), mbCameraUpdated(false), mLast mReferencePoints.color.r =1.0f; mReferencePoints.color.a = 1.0; + mbLastMapUpdateIdx = mpMap->GetMapUpdateIdx() - 1; //decrease in order to force an update + //Configure Publisher - mMapPublisher = nh.advertise("ORB_SLAM/Map", 10); - mPosePublisher = nh.advertise("ORB_SLAM/Pose", 10); - - mMapPublisher.publish(mPoints); - mMapPublisher.publish(mReferencePoints); - mMapPublisher.publish(mCovisibilityGraph); - mMapPublisher.publish(mKeyFrames); - mMapPublisher.publish(mCurrentCamera); + publisher = nh.advertise("ORB_SLAM/Map", 10); + + publisher.publish(mPoints); + publisher.publish(mReferencePoints); + publisher.publish(mCovisibilityGraph); + publisher.publish(mKeyFrames); + publisher.publish(mCurrentCamera); } void MapPublisher::Refresh() { + //save computation if their is no subscriber + if(publisher.getNumSubscribers() > 0 ) + { if(isCamUpdated()) { cv::Mat Tcw = GetCurrentCameraPose(); PublishCurrentCamera(Tcw); ResetCamFlag(); } - if(mpMap->isMapUpdated()) + if(mpMap->isMapUpdated(mbLastMapUpdateIdx)) { - vector vKeyFrames = mpMap->GetAllKeyFrames(); - vector vMapPoints = mpMap->GetAllMapPoints(); - vector vRefMapPoints = mpMap->GetReferenceMapPoints(); + vector> vKeyFrames = mpMap->GetAllKeyFrames(); + vector> vMapPoints = mpMap->GetAllMapPoints(); + vector> vRefMapPoints = mpMap->GetReferenceMapPoints(); PublishMapPoints(vMapPoints, vRefMapPoints); PublishKeyFrames(vKeyFrames); - mpMap->ResetUpdated(); + mbLastMapUpdateIdx = mpMap->GetMapUpdateIdx(); } + } } -void MapPublisher::PublishMapPoints(const vector &vpMPs, const vector &vpRefMPs) +void MapPublisher::PublishMapPoints(const vector> &vpMPs, const vector> &vpRefMPs) { mPoints.points.clear(); mReferencePoints.points.clear(); - set spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); + set> spRefMPs(vpRefMPs.begin(), vpRefMPs.end()); for(size_t i=0, iend=vpMPs.size(); i &vpMPs, const vector mPoints.points.push_back(p); } - for(set::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++) + for(set>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++) { if((*sit)->isBad()) continue; @@ -173,11 +178,11 @@ void MapPublisher::PublishMapPoints(const vector &vpMPs, const vector mPoints.header.stamp = ros::Time::now(); mReferencePoints.header.stamp = ros::Time::now(); - mMapPublisher.publish(mPoints); - mMapPublisher.publish(mReferencePoints); + publisher.publish(mPoints); + publisher.publish(mReferencePoints); } -void MapPublisher::PublishKeyFrames(const vector &vpKFs) +void MapPublisher::PublishKeyFrames(const vector> &vpKFs) { mKeyFrames.points.clear(); mCovisibilityGraph.points.clear(); @@ -237,10 +242,10 @@ void MapPublisher::PublishKeyFrames(const vector &vpKFs) mKeyFrames.points.push_back(msgs_p1); // Covisibility Graph - vector vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); + vector> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); if(!vCovKFs.empty()) { - for(vector::iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++) + for(vector>::iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++) { if((*vit)->mnIdmnId) continue; @@ -255,7 +260,7 @@ void MapPublisher::PublishKeyFrames(const vector &vpKFs) } // MST - KeyFrame* pParent = vpKFs[i]->GetParent(); + std::shared_ptr pParent = vpKFs[i]->GetParent(); if(pParent) { cv::Mat Owp = pParent->GetCameraCenter(); @@ -266,8 +271,8 @@ void MapPublisher::PublishKeyFrames(const vector &vpKFs) mMST.points.push_back(msgs_o); mMST.points.push_back(msgs_op); } - set sLoopKFs = vpKFs[i]->GetLoopEdges(); - for(set::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++) + set> sLoopKFs = vpKFs[i]->GetLoopEdges(); + for(set>::iterator sit=sLoopKFs.begin(), send=sLoopKFs.end(); sit!=send; sit++) { if((*sit)->mnIdmnId) continue; @@ -285,9 +290,9 @@ void MapPublisher::PublishKeyFrames(const vector &vpKFs) mCovisibilityGraph.header.stamp = ros::Time::now(); mMST.header.stamp = ros::Time::now(); - mMapPublisher.publish(mKeyFrames); - mMapPublisher.publish(mCovisibilityGraph); - mMapPublisher.publish(mMST); + publisher.publish(mKeyFrames); + publisher.publish(mCovisibilityGraph); + publisher.publish(mMST); } void MapPublisher::PublishCurrentCamera(const cv::Mat &Tcw) @@ -310,7 +315,7 @@ void MapPublisher::PublishCurrentCamera(const cv::Mat &Tcw) cv::Mat p3w = Twc*p3; cv::Mat p4w = Twc*p4; - geometry_msgs::Point msgs_o, msgs_p1, msgs_p2, msgs_p3, msgs_p4; + geometry_msgs::Point msgs_o,msgs_p1, msgs_p2, msgs_p3, msgs_p4; msgs_o.x=ow.at(0); msgs_o.y=ow.at(1); msgs_o.z=ow.at(2); @@ -345,97 +350,15 @@ void MapPublisher::PublishCurrentCamera(const cv::Mat &Tcw) mCurrentCamera.points.push_back(msgs_p1); mCurrentCamera.header.stamp = ros::Time::now(); - mMapPublisher.publish(mCurrentCamera); - // Pose publisher - if (mRangeScale > 0 && mPosePublisher.getNumSubscribers() > 0) - { - geometry_msgs::PoseStamped pose; - cv::Mat Rwc = (Twc.rowRange(0,3).colRange(0,3)).t(); - vector q = ORB_SLAM::Converter::toQuaternion(Rwc); - geometry_msgs::Quaternion msg_q; - msg_q.x = q[0]; - msg_q.y = q[1]; - msg_q.z = q[2]; - msg_q.w = q[3]; - - msgs_o.x = msgs_o.x * mRangeScale; - msgs_o.y = msgs_o.y * mRangeScale; - msgs_o.z = msgs_o.z * mRangeScale; - - pose.header.stamp = ros::Time::now(); - pose.header.frame_id = "/ORB_SLAM/World"; - pose.pose.position = msgs_o; - pose.pose.orientation = msg_q; - mPosePublisher.publish(pose); - } + publisher.publish(mCurrentCamera); } -void MapPublisher::SetCurrentCameraPose(const cv::Mat &Tcw, const double &poseStamp, const float &range, const double &rangeStamp) +void MapPublisher::SetCurrentCameraPose(const cv::Mat &Tcw) { boost::mutex::scoped_lock lock(mMutexCamera); mCameraPose = Tcw.clone(); mbCameraUpdated = true; - - // Current camera pose at range axis - cv::Mat o = (cv::Mat_(4,1) << 0, 0, 0, 1); - cv::Mat Twc = Tcw.inv(); - cv::Mat ow = Twc*o; - float pose = ow.at(2); - - // Compute scale only once - if (mRangeScale > 0) - return; - - // Scale computation - const double eps = 1e-2; - int idx = -1; - for (uint i=0; i= 0) - { - if (mLastStamp < 0 ) - { - // First iteration - mLastPose = mZposes[idx].first; - mLastStamp = mZposes[idx].second; - mLastRange = range; - } - else - { - // Minimum time between scale updates - if ( (mZposes[idx].second - mLastStamp) > 2.0) - { - float poseDiff = fabs(mZposes[idx].first - mLastPose); - float rangeDiff = fabs(range - mLastRange); - - // Check enough range difference - if (rangeDiff > 0.2) - { - // Set scale - mRangeScale = rangeDiff / poseDiff; - ROS_INFO_STREAM("Scale set to: " << mRangeScale); - - // Save - mLastPose = mZposes[idx].first; - mLastStamp = mZposes[idx].second; - mLastRange = range; - } - } - } - - mZposes.clear(); - } - - // Save - mZposes.push_back(make_pair(pose, poseStamp)); } cv::Mat MapPublisher::GetCurrentCameraPose() @@ -456,9 +379,4 @@ void MapPublisher::ResetCamFlag() mbCameraUpdated = false; } -float MapPublisher::GetRangeScale() -{ - return mRangeScale; -} - } //namespace ORB_SLAM diff --git a/orb_slam/src/ORBextractor.cc b/orb_slam/src/ORBextractor.cc index 01f4d304..0188357d 100644 --- a/orb_slam/src/ORBextractor.cc +++ b/orb_slam/src/ORBextractor.cc @@ -57,6 +57,8 @@ #include #include +#include +#include #include #include "ORBextractor.h" @@ -716,7 +718,7 @@ static void computeDescriptors(const Mat& image, vector& keypoints, Ma void ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints, OutputArray _descriptors) -{ +{ if(_image.empty()) return; diff --git a/orb_slam/src/ORBmatcher.cc b/orb_slam/src/ORBmatcher.cc index cc7835eb..dc69092c 100644 --- a/orb_slam/src/ORBmatcher.cc +++ b/orb_slam/src/ORBmatcher.cc @@ -21,15 +21,15 @@ #include "ORBmatcher.h" -#include +#include -#include -#include -#include +#include +#include +#include #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" -#include +#include using namespace std; @@ -46,7 +46,7 @@ ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbChec { } -int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th) +int ORBmatcher::SearchByProjection(Frame &F, const vector> &vpMapPoints, const float th) { int nmatches=0; @@ -54,14 +54,14 @@ int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoint for(size_t iMP=0; iMP pMP = vpMapPoints[iMP]; if(!pMP->mbTrackInView) continue; if(pMP->isBad()) continue; - const int &nPredictedLevel = pMP->mnTrackScaleLevel; + const int &nPredictedLevel = pMP->mnTrackScaleLevel; // The size of the window will depend on the viewing direction float r = RadiusByViewingCos(pMP->mTrackViewCos); @@ -77,9 +77,9 @@ int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoint cv::Mat MPdescriptor = pMP->GetDescriptor(); - int bestDist = INT_MAX; - int bestLevel = -1; - int bestDist2 = INT_MAX; + int bestDist=INT_MAX; + int bestLevel= -1; + int bestDist2=INT_MAX; int bestLevel2 = -1; int bestIdx =-1 ; @@ -91,22 +91,22 @@ int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoint if(F.mvpMapPoints[idx]) continue; - cv::Mat d = F.mDescriptors.row(idx); + cv::Mat d=F.mDescriptors.row(idx); const int dist = DescriptorDistance(MPdescriptor,d); if(dist &vpMapPoint if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2) continue; - F.mvpMapPoints[bestIdx] = pMP; + F.mvpMapPoints[bestIdx]=pMP; nmatches++; } } @@ -133,7 +133,7 @@ float ORBmatcher::RadiusByViewingCos(const float &viewCos) } -bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2) +bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const std::shared_ptr pKF2) { // Epipolar line in second image l = x1'F12 = [a b c] const float a = kp1.pt.x*F12.at(0,0)+kp1.pt.y*F12.at(1,0)+F12.at(2,0); @@ -152,11 +152,11 @@ bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoin return dsqr<3.84*pKF2->GetSigma2(kp2.octave); } -int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPointMatches) +int ORBmatcher::SearchByBoW(std::shared_ptr pKF,Frame &F, vector> &vpMapPointMatches) { - vector vpMapPointsKF = pKF->GetMapPointMatches(); + vector> vpMapPointsKF = pKF->GetMapPointMatches(); - vpMapPointMatches = vector(F.mvpMapPoints.size(),static_cast(NULL)); + vpMapPointMatches = vector>(F.mvpMapPoints.size(),static_cast>(NULL)); DBoW2::FeatureVector vFeatVecKF = pKF->GetFeatureVector(); @@ -184,7 +184,7 @@ int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPoin { const unsigned int realIdxKF = vIndicesKF[iKF]; - MapPoint* pMP = vpMapPointsKF[realIdxKF]; + std::shared_ptr pMP = vpMapPointsKF[realIdxKF]; if(!pMP) continue; @@ -283,7 +283,7 @@ int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPoin return nmatches; } -int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector &vpPoints, vector &vpMatched, int th) +int ORBmatcher::SearchByProjection(std::shared_ptr pKF, cv::Mat Scw, const vector> &vpPoints, vector> &vpMatched, int th) { // Get Calibration Parameters for later projection const float fx = pKF->fx; @@ -302,7 +302,7 @@ int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector spAlreadyFound(vpMatched.begin(), vpMatched.end()); + set> spAlreadyFound(vpMatched.begin(), vpMatched.end()); spAlreadyFound.erase(NULL); int nmatches=0; @@ -310,7 +310,7 @@ int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector pMP = vpPoints[iMP]; // Discard Bad MapPoints and already found if(pMP->isBad() || spAlreadyFound.count(pMP)) @@ -406,10 +406,10 @@ int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector &vpMapPointMatches2, int minScaleLevel, int maxScaleLevel) +int ORBmatcher::WindowSearch(Frame &F1, Frame &F2, int windowSize, vector> &vpMapPointMatches2, int minScaleLevel, int maxScaleLevel) { int nmatches=0; - vpMapPointMatches2 = vector(F2.mvpMapPoints.size(),static_cast(NULL)); + vpMapPointMatches2 = vector>(F2.mvpMapPoints.size(),static_cast>(NULL)); vector vnMatches21 = vector(F2.mvKeysUn.size(),-1); vector rotHist[HISTO_LENGTH]; @@ -418,11 +418,11 @@ int ORBmatcher::WindowSearch(Frame &F1, Frame &F2, int windowSize, vector0; - const bool bMaxLevel = maxScaleLevel pMP1 = F1.mvpMapPoints[i1]; if(!pMP1) continue; @@ -464,9 +464,9 @@ int ORBmatcher::WindowSearch(Frame &F1, Frame &F2, int windowSize, vector &vpMapPointMatches2) +int ORBmatcher::SearchByProjection(Frame &F1, Frame &F2, int windowSize, vector> &vpMapPointMatches2) { vpMapPointMatches2 = F2.mvpMapPoints; - set spMapPointsAlreadyFound(vpMapPointMatches2.begin(),vpMapPointMatches2.end()); + set> spMapPointsAlreadyFound(vpMapPointMatches2.begin(),vpMapPointMatches2.end()); int nmatches = 0; @@ -528,7 +528,7 @@ int ORBmatcher::SearchByProjection(Frame &F1, Frame &F2, int windowSize, vector< for(size_t i1=0, iend1=F1.mvpMapPoints.size(); i1 pMP1 = F1.mvpMapPoints[i1]; if(!pMP1) continue; @@ -712,19 +712,19 @@ int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector &vpMatches12) +int ORBmatcher::SearchByBoW(std::shared_ptrpKF1, std::shared_ptrpKF2, vector> &vpMatches12) { vector vKeysUn1 = pKF1->GetKeyPointsUn(); DBoW2::FeatureVector vFeatVec1 = pKF1->GetFeatureVector(); - vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector> vpMapPoints1 = pKF1->GetMapPointMatches(); cv::Mat Descriptors1 = pKF1->GetDescriptors(); vector vKeysUn2 = pKF2->GetKeyPointsUn(); DBoW2::FeatureVector vFeatVec2 = pKF2->GetFeatureVector(); - vector vpMapPoints2 = pKF2->GetMapPointMatches(); + vector> vpMapPoints2 = pKF2->GetMapPointMatches(); cv::Mat Descriptors2 = pKF2->GetDescriptors(); - vpMatches12 = vector(vpMapPoints1.size(),static_cast(NULL)); + vpMatches12 = vector>(vpMapPoints1.size(),static_cast>(NULL)); vector vbMatched2(vpMapPoints2.size(),false); vector rotHist[HISTO_LENGTH]; @@ -748,7 +748,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector & { size_t idx1 = f1it->second[i1]; - MapPoint* pMP1 = vpMapPoints1[idx1]; + std::shared_ptr pMP1 = vpMapPoints1[idx1]; if(!pMP1) continue; if(pMP1->isBad()) @@ -764,7 +764,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector & { size_t idx2 = f2it->second[i2]; - MapPoint* pMP2 = vpMapPoints2[idx2]; + std::shared_ptr pMP2 = vpMapPoints2[idx2]; if(vbMatched2[idx2] || !pMP2) continue; @@ -849,15 +849,15 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector & return nmatches; } -int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12, +int ORBmatcher::SearchForTriangulation(std::shared_ptrpKF1, std::shared_ptrpKF2, cv::Mat F12, vector &vMatchedKeys1, vector &vMatchedKeys2, vector > &vMatchedPairs) { - vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector> vpMapPoints1 = pKF1->GetMapPointMatches(); vector vKeysUn1 = pKF1->GetKeyPointsUn(); cv::Mat Descriptors1 = pKF1->GetDescriptors(); DBoW2::FeatureVector vFeatVec1 = pKF1->GetFeatureVector(); - vector vpMapPoints2 = pKF2->GetMapPointMatches(); + vector> vpMapPoints2 = pKF2->GetMapPointMatches(); vector vKeysUn2 = pKF2->GetKeyPointsUn(); cv::Mat Descriptors2 = pKF2->GetDescriptors(); DBoW2::FeatureVector vFeatVec2 = pKF2->GetFeatureVector(); @@ -889,7 +889,7 @@ vector &vMatchedKeys1, vector &vMatchedKeys2, vector { size_t idx1 = f1it->second[i1]; - MapPoint* pMP1 = vpMapPoints1[idx1]; + std::shared_ptr pMP1 = vpMapPoints1[idx1]; // If there is already a MapPoint skip if(pMP1) @@ -905,7 +905,7 @@ vector &vMatchedKeys1, vector &vMatchedKeys2, vector { size_t idx2 = f2it->second[i2]; - MapPoint* pMP2 = vpMapPoints2[idx2]; + std::shared_ptr pMP2 = vpMapPoints2[idx2]; // If we have already matched or there is a MapPoint skip if(vbMatched2[idx2] || pMP2) @@ -1013,7 +1013,7 @@ vector &vMatchedKeys1, vector &vMatchedKeys2, vector return nmatches; } -int ORBmatcher::Fuse(KeyFrame *pKF, vector &vpMapPoints, float th) +int ORBmatcher::Fuse(std::shared_ptrpKF, vector> &vpMapPoints, float th) { cv::Mat Rcw = pKF->GetRotation(); cv::Mat tcw = pKF->GetTranslation(); @@ -1032,7 +1032,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, vector &vpMapPoints, float th) for(size_t i=0; i pMP = vpMapPoints[i]; if(!pMP) continue; @@ -1115,11 +1115,11 @@ int ORBmatcher::Fuse(KeyFrame *pKF, vector &vpMapPoints, float th) // If there is already a MapPoint replace otherwise add new measurement if(bestDist<=TH_LOW) { - MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx); + std::shared_ptr pMPinKF = pKF->GetMapPoint(bestIdx); if(pMPinKF) { if(!pMPinKF->isBad()) - pMP->Replace(pMPinKF); + pMP->Replace(pMPinKF); } else { @@ -1133,7 +1133,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, vector &vpMapPoints, float th) return nFused; } -int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoints, float th) +int ORBmatcher::Fuse(std::shared_ptrpKF, cv::Mat Scw, const vector> &vpPoints, float th) { // Get Calibration Parameters for later projection const float &fx = pKF->fx; @@ -1149,7 +1149,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoi cv::Mat Ow = -Rcw.t()*tcw; // Set of MapPoints already found in the KeyFrame - set spAlreadyFound = pKF->GetMapPoints(); + set> spAlreadyFound = pKF->GetMapPoints(); const int nMaxLevel = pKF->GetScaleLevels()-1; vector vfScaleFactors = pKF->GetScaleFactors(); @@ -1159,7 +1159,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoi // For each candidate MapPoint project and match for(size_t iMP=0, iendMP=vpPoints.size(); iMP pMP = vpPoints[iMP]; // Discard Bad MapPoints and already found if(pMP->isBad() || spAlreadyFound.count(pMP)) @@ -1244,7 +1244,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoi // If there is already a MapPoint replace otherwise add new measurement if(bestDist<=TH_LOW) { - MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx); + std::shared_ptr pMPinKF = pKF->GetMapPoint(bestIdx); if(pMPinKF) { if(!pMPinKF->isBad()) @@ -1264,7 +1264,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoi } -int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12, +int ORBmatcher::SearchBySim3(std::shared_ptrpKF1, std::shared_ptrpKF2, vector> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, float th) { const float fx = pKF1->fx; @@ -1288,13 +1288,13 @@ int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & const int nMaxLevel1 = pKF1->GetScaleLevels()-1; vector vfScaleFactors1 = pKF1->GetScaleFactors(); - vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector> vpMapPoints1 = pKF1->GetMapPointMatches(); const int N1 = vpMapPoints1.size(); const int nMaxLevel2 = pKF2->GetScaleLevels()-1; vector vfScaleFactors2 = pKF2->GetScaleFactors(); - vector vpMapPoints2 = pKF2->GetMapPointMatches(); + vector> vpMapPoints2 = pKF2->GetMapPointMatches(); const int N2 = vpMapPoints2.size(); vector vbAlreadyMatched1(N1,false); @@ -1302,7 +1302,7 @@ int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & for(int i=0; i pMP = vpMatches12[i]; if(pMP) { vbAlreadyMatched1[i]=true; @@ -1318,7 +1318,7 @@ int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & // Transform from KF1 to KF2 and search for(int i1=0; i1 pMP = vpMapPoints1[i1]; if(!pMP || vbAlreadyMatched1[i1]) continue; @@ -1401,7 +1401,7 @@ int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & // Transform from KF2 to KF2 and search for(int i2=0; i2 pMP = vpMapPoints2[i2]; if(!pMP || vbAlreadyMatched2[i2]) continue; @@ -1519,7 +1519,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, for(size_t i=0, iend=LastFrame.mvpMapPoints.size(); i pMP = LastFrame.mvpMapPoints[i]; if(pMP) { @@ -1592,7 +1592,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, } } } - } + } //Apply rotation consistency if(mbCheckOrientation) @@ -1619,7 +1619,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, return nmatches; } -int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set &sAlreadyFound, float th ,int ORBdist) +int ORBmatcher::SearchByProjection(Frame &CurrentFrame, std::shared_ptrpKF, const set> &sAlreadyFound, float th ,int ORBdist) { int nmatches = 0; @@ -1633,11 +1633,11 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set rotHist[i].reserve(500); const float factor = 1.0f/HISTO_LENGTH; - vector vpMPs = pKF->GetMapPointMatches(); + vector> vpMPs = pKF->GetMapPointMatches(); for(size_t i=0, iend=vpMPs.size(); i pMP = vpMPs[i]; if(pMP) { diff --git a/orb_slam/src/OctoMapPublisher.cc b/orb_slam/src/OctoMapPublisher.cc new file mode 100644 index 00000000..03c0de88 --- /dev/null +++ b/orb_slam/src/OctoMapPublisher.cc @@ -0,0 +1,382 @@ +/** +* This file is part of ORB-SLAM. +* +* Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM. If not, see . +*/ + +#include "OctoMapPublisher.h" +#include "MapPoint.h" +#include "KeyFrame.h" +#include +#include +#include +#include +#include +#include +#include + +#define DEFAULT_OCTOMAP_RESOLUTION 0.5 + +#define PROJECTION_MIN_HEIGHT 0.01 + + +namespace ORB_SLAM +{ + +using namespace octomap; + +void OctoMapPublisher::reset(octomap::OcTree& octoMap) +{ + octoMap.clear(); + mbLastMapUpdateIdx = mpMap->GetMapUpdateIdx() - 1; //decrease in order to force an update +// ParameterServer* ps = ParameterServer::instance(); +// m_octoMap.setClampingThresMin(ps->get("octomap_clamping_min")); +// m_octoMap.setClampingThresMax(ps->get("octomap_clamping_max")); +// m_octoMap.setResolution(ps->get("octomap_resolution")); +// m_octoMap.setOccupancyThres(ps->get("octomap_occupancy_threshold")); +// m_octoMap.setProbHit(ps->get("octomap_prob_hit")); +// m_octoMap.setProbMiss(ps->get("octomap_prob_miss")); + +} + +bool OctoMapPublisher::octomapBinarySrv(octomap_msgs::GetOctomapRequest &req, + octomap_msgs::GetOctomapResponse &res) +{ + static octomap::OcTree octoMap(m_octomapResolution); + refreshMap(octoMap); + ros::WallTime startTime = ros::WallTime::now(); + ROS_INFO("Sending binary map data on service request"); + res.map.header.frame_id = MAP_FRAME_ID; + res.map.header.stamp = ros::Time::now(); + if (!octomap_msgs::binaryMapToMsg(octoMap, res.map)){ + return false; + } + + double total_elapsed = (ros::WallTime::now() - startTime).toSec(); + ROS_INFO("Binary octomap sent in %f sec", total_elapsed); + return true; +} + +bool OctoMapPublisher::octomapFullSrv(octomap_msgs::GetOctomapRequest &req, + octomap_msgs::GetOctomapResponse &res) +{ + static octomap::OcTree octoMap(m_octomapResolution); + refreshMap(octoMap); + ROS_INFO("Sending full map data on service request"); + res.map.header.frame_id = MAP_FRAME_ID; + res.map.header.stamp = ros::Time::now(); + + if (!octomap_msgs::fullMapToMsg(octoMap, res.map)){ + return false; + } + + return true; +} + +bool OctoMapPublisher::save(string filename) +{ + static octomap::OcTree octoMap(m_octomapResolution); + refreshMap(octoMap); + + filename.append(".ot"); + + std::ofstream outfile(filename.c_str(), std::ios_base::out | std::ios_base::binary); + if (outfile.is_open()){ + ROS_DEBUG("Writing octomap to %s", filename.c_str()); + octoMap.write(outfile); + outfile.close(); + ROS_DEBUG("color tree written %s", filename.c_str()); + return true; + } + else { + ROS_ERROR("could not open %s for writing", filename.c_str()); + return false; + } +} + +bool OctoMapPublisher::octomapSaveSrv(orb_slam::SaveOctomapRequest &req, + orb_slam::SaveOctomapResponse &res) +{ + return save(req.filename); +} + +bool OctoMapPublisher::occupancySrv(nav_msgs::GetMapRequest &req, + nav_msgs::GetMapResponse &res) +{ + static octomap::OcTree octoMap(m_octomapResolution); + refreshMap(octoMap); + ROS_INFO("Sending full map data on service request"); + res.map.header.frame_id = MAP_FRAME_ID; + res.map.header.stamp = ros::Time::now(); + + //XXX no fixed limits + octomapToOccupancyGrid(octoMap, res.map, m_projectionMinHeight, std::numeric_limits::max()); + + return true; +} + + +OctoMapPublisher::OctoMapPublisher(Map* pMap):nh("~"), + m_projectionMinHeight(PROJECTION_MIN_HEIGHT), + m_octomapResolution(DEFAULT_OCTOMAP_RESOLUTION), + mpMap(pMap), + mbLastMapUpdateIdx(0), + MAP_FRAME_ID("/orb_slam/map"), + CAMERA_FRAME_ID("/ORB_SLAM/Camera"), + BASE_LINK_FRAME_ID("ORB_base_link") +{ + + nh.param("occupancy_projection_min_height", m_projectionMinHeight, m_projectionMinHeight); + nh.param("octomap_resolution", m_octomapResolution, m_octomapResolution); + + mbLastMapUpdateIdx = mpMap->GetMapUpdateIdx()-1; //decrease in order to force an update + + //Configure Publisher + publisherPCL = nh.advertise("point_cloud", 10); + publisherOctomapFull = nh.advertise("octomap_full", 10); + publisherOctomapBinary = nh.advertise("octomap_binary", 10); + publisherProjected = nh.advertise("projected_map", 5, 10); + + //Configure Services + m_octomapSaveService = nh.advertiseService("save_octomap", &OctoMapPublisher::octomapSaveSrv, this); + m_octomapBinaryService = nh.advertiseService("octomap_binary", &OctoMapPublisher::octomapBinarySrv, this); + m_octomapFullService = nh.advertiseService("octomap_full", &OctoMapPublisher::octomapFullSrv, this); + m_occupancyService = nh.advertiseService("occupancy_grid", &OctoMapPublisher::occupancySrv, this); +} + +void OctoMapPublisher::octomapToOccupancyGrid(const octomap::OcTree& octree, nav_msgs::OccupancyGrid& map, const double minZ_, const double maxZ_ ) +{ + map.info.resolution = octree.getResolution(); + double minX, minY, minZ; + double maxX, maxY, maxZ; + octree.getMetricMin(minX, minY, minZ); + octree.getMetricMax(maxX, maxY, maxZ); + ROS_DEBUG("Octree min %f %f %f", minX, minY, minZ); + ROS_DEBUG("Octree max %f %f %f", maxX, maxY, maxZ); + minZ = std::max(minZ_, minZ); + maxZ = std::min(maxZ_, maxZ); + + octomap::point3d minPt(minX, minY, minZ); + octomap::point3d maxPt(maxX, maxY, maxZ); + octomap::OcTreeKey minKey, maxKey, curKey; + + if (!octree.coordToKeyChecked(minPt, minKey)) + { + ROS_ERROR("Could not create OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); + return; + } + if (!octree.coordToKeyChecked(maxPt, maxKey)) + { + ROS_ERROR("Could not create OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); + return; + } + + map.info.width = maxKey[0] - minKey[0] + 1; + map.info.height = maxKey[1] - minKey[1] + 1; + + // might not exactly be min / max: + octomap::point3d origin = octree.keyToCoord(minKey, octree.getTreeDepth()); + map.info.origin.position.x = origin.x() - octree.getResolution() * 0.5; + map.info.origin.position.y = origin.y() - octree.getResolution() * 0.5; + + map.info.origin.orientation.x = 0.; + map.info.origin.orientation.y = 0.; + map.info.origin.orientation.z = 0.; + map.info.origin.orientation.w = 1.; + + // Allocate space to hold the data + map.data.resize(map.info.width * map.info.height, -1); + + //init with unknown + for(std::vector::iterator it = map.data.begin(); it != map.data.end(); ++it) { + *it = -1; + } + + // iterate over all keys: + unsigned i, j; + for (curKey[1] = minKey[1], j = 0; curKey[1] <= maxKey[1]; ++curKey[1], ++j) + { + for (curKey[0] = minKey[0], i = 0; curKey[0] <= maxKey[0]; ++curKey[0], ++i) + { + for (curKey[2] = minKey[2]; curKey[2] <= maxKey[2]; ++curKey[2]) + { //iterate over height + octomap::OcTreeNode* node = octree.search(curKey); + if (node) + { + bool occupied = octree.isNodeOccupied(node); + if(occupied){ + map.data[map.info.width * j + i] = 100; + break; + }else{ + map.data[map.info.width * j + i] = 0; + } + } + } + } + } +} + +void OctoMapPublisher::publishProjectedMap() +{ + static nav_msgs::OccupancyGrid msgOccupancy; + if (publisherProjected.getNumSubscribers() > 0) + { + static octomap::OcTree octoMap(m_octomapResolution); + ros::Time now = ros::Time::now(); + refreshMap(octoMap); + msgOccupancy.header.frame_id = MAP_FRAME_ID; + msgOccupancy.header.stamp = now; + + octomapToOccupancyGrid(octoMap, msgOccupancy, m_projectionMinHeight, std::numeric_limits::max()); + + publisherProjected.publish(msgOccupancy); + + } +} + +void OctoMapPublisher::publishPointCloud() +{ + static sensor_msgs::PointCloud2 msgPointCloud; + //save computation if their is no subscriber + if (publisherPCL.getNumSubscribers() > 0) + { + pcl::PointCloud pclCloud; + vector> vMapPoints = mpMap->GetAllMapPoints(); + vector> vRefMapPoints = mpMap->GetReferenceMapPoints(); + mapPointsToPCL(vRefMapPoints, pclCloud); + pcl::toROSMsg(pclCloud, msgPointCloud); + msgPointCloud.header.frame_id = CAMERA_FRAME_ID; + msgPointCloud.header.stamp = ros::Time::now(); + msgPointCloud.header.seq++; + publisherPCL.publish(msgPointCloud); + } +} + +void OctoMapPublisher::publishOctomap() +{ + if (publisherOctomapBinary.getNumSubscribers() > 0 || publisherOctomapFull.getNumSubscribers() > 0) + { + static octomap::OcTree octoMap(m_octomapResolution); + ros::Time now = ros::Time::now(); + refreshMap(octoMap); + + if (publisherOctomapBinary.getNumSubscribers() > 0) + { + static octomap_msgs::Octomap msgOctomapBinary; + msgOctomapBinary.header.frame_id = MAP_FRAME_ID; + msgOctomapBinary.header.stamp = now; + + if (octomap_msgs::binaryMapToMsg(octoMap, msgOctomapBinary)) + { + publisherOctomapBinary.publish(msgOctomapBinary); + } + } + if (publisherOctomapFull.getNumSubscribers() > 0) + { + static octomap_msgs::Octomap msgOctomapFull; + msgOctomapFull.header.frame_id = MAP_FRAME_ID; + msgOctomapFull.header.stamp = now; + if (octomap_msgs::fullMapToMsg(octoMap, msgOctomapFull)) + { + publisherOctomapFull.publish(msgOctomapFull); + } + } + } +} + +void OctoMapPublisher::Publish() +{ + //XXX think about special treatment + publishPointCloud(); + + if(mpMap->isMapUpdated(mbLastMapUpdateIdx)) + { + publishOctomap(); + + publishProjectedMap(); + + //reset update indicator + mbLastMapUpdateIdx = mpMap->GetMapUpdateIdx(); + } +} + +void OctoMapPublisher::refreshMap(octomap::OcTree& octoMap) +{ + reset(octoMap); + vector> vMapPoints = mpMap->GetAllMapPoints(); + + mapPointsToOctomap(vMapPoints, octoMap); +} + +void OctoMapPublisher::mapPointsToOctomap(const vector> &vpMPs, octomap::OcTree& octoMap) +{ + Pointcloud pointCloud; + + for(size_t i=0, iend=vpMPs.size(); iisBad()){ + continue; + } + cv::Mat pos = vpMPs[i]->GetWorldPos(); + + pointCloud.push_back(pos.at(0), pos.at(1),pos.at(2)); + } + + if(pointCloud.size() == 0) + { + return; + } + + octomap::point3d origin; + + try{ + + tf::StampedTransform transform_in_target_frame; + + m_tf_listener.lookupTransform(BASE_LINK_FRAME_ID, CAMERA_FRAME_ID, ros::Time(0) , transform_in_target_frame); + + octomap::pose6d frame = octomap::poseTfToOctomap(transform_in_target_frame); + + //TODO test discretize=true + //TODO potential option insert only current like used for point cloud and integrate octomap + octoMap.insertPointCloud(pointCloud, origin, frame, -1, false, false); + }catch(...){ + ROS_ERROR("TF from %s to %s not available for point cloud generation", BASE_LINK_FRAME_ID, CAMERA_FRAME_ID); + octoMap.insertPointCloud(pointCloud, origin, -1, false, false); + } + +} + +void OctoMapPublisher::mapPointsToPCL(const vector> &vpRefMPs, pcl::PointCloud& pclCloud) +{ + //only current (local) mapping points are taking into account + for(size_t i=0, iend=vpRefMPs.size(); iisBad()) + { + continue; + } + cv::Mat pos = vpRefMPs[i]->GetWorldPos(); + + pcl::PointXYZ point(pos.at(0), pos.at(1), pos.at(2)); + pclCloud.points.push_back(point); + } + pclCloud.height = 1; //unstructured cloud + pclCloud.width = pclCloud.points.size(); +} + +} //namespace ORB_SLAM diff --git a/orb_slam/src/Optimizer.cc b/orb_slam/src/Optimizer.cc index 3f4f7538..ea2b4c2e 100644 --- a/orb_slam/src/Optimizer.cc +++ b/orb_slam/src/Optimizer.cc @@ -37,13 +37,13 @@ namespace ORB_SLAM void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag) { - vector vpKFs = pMap->GetAllKeyFrames(); - vector vpMP = pMap->GetAllMapPoints(); + vector> vpKFs = pMap->GetAllKeyFrames(); + vector> vpMP = pMap->GetAllMapPoints(); BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag); } -void Optimizer::BundleAdjustment(const vector &vpKFs, const vector &vpMP, int nIterations, bool* pbStopFlag) +void Optimizer::BundleAdjustment(const vector> &vpKFs, const vector> &vpMP, int nIterations, bool* pbStopFlag) { g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType * linearSolver; @@ -63,7 +63,7 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector pKF = vpKFs[i]; if(pKF->isBad()) continue; g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); @@ -80,7 +80,7 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector pMP = vpMP[i]; if(pMP->isBad()) continue; g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); @@ -90,12 +90,12 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vectorsetMarginalized(true); optimizer.addVertex(vPoint); - map observations = pMP->GetObservations(); + map,size_t> observations = pMP->GetObservations(); //SET EDGES - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; if(pKF->isBad()) continue; Eigen::Matrix obs; @@ -133,7 +133,7 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector pKF = vpKFs[i]; g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); pKF->SetPose(Converter::toCvMat(SE3quat)); @@ -142,7 +142,7 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector pMP = vpMP[i]; g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate())); pMP->UpdateNormalAndDepth(); @@ -189,7 +189,7 @@ int Optimizer::PoseOptimization(Frame *pFrame) for(int i=0; imvpMapPoints[i]; + std::shared_ptr pMP = pFrame->mvpMapPoints[i]; if(pMP) { g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); @@ -283,31 +283,31 @@ int Optimizer::PoseOptimization(Frame *pFrame) return nInitialCorrespondences-nBad; } -void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) +void Optimizer::LocalBundleAdjustment(std::shared_ptrpKF, bool* pbStopFlag) { // Local KeyFrames: First Breath Search from Current Keyframe - list lLocalKeyFrames; + list> lLocalKeyFrames; lLocalKeyFrames.push_back(pKF); pKF->mnBALocalForKF = pKF->mnId; - vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); + vector> vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); for(int i=0, iend=vNeighKFs.size(); i pKFi = vNeighKFs[i]; pKFi->mnBALocalForKF = pKF->mnId; if(!pKFi->isBad()) lLocalKeyFrames.push_back(pKFi); } // Local MapPoints seen in Local KeyFrames - list lLocalMapPoints; - for(list::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++) + list> lLocalMapPoints; + for(list>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++) { - vector vpMPs = (*lit)->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + vector> vpMPs = (*lit)->GetMapPointMatches(); + for(vector>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) { - MapPoint* pMP = *vit; + std::shared_ptr pMP = *vit; if(pMP) if(!pMP->isBad()) if(pMP->mnBALocalForKF!=pKF->mnId) @@ -319,13 +319,13 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) } // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes - list lFixedCameras; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + list> lFixedCameras; + for(list>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { - map observations = (*lit)->GetObservations(); - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + map,size_t> observations = (*lit)->GetObservations(); + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKFi = mit->first; + std::shared_ptr pKFi = mit->first; if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId) { @@ -353,9 +353,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) long unsigned int maxKFid = 0; // SET LOCAL KEYFRAME VERTICES - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) { - KeyFrame* pKFi = *lit; + std::shared_ptr pKFi = *lit; g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose())); vSE3->setId(pKFi->mnId); @@ -366,9 +366,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) } // SET FIXED KEYFRAME VERTICES - for(list::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++) + for(list>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++) { - KeyFrame* pKFi = *lit; + std::shared_ptr pKFi = *lit; g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose())); vSE3->setId(pKFi->mnId); @@ -384,20 +384,20 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) vector vpEdges; vpEdges.reserve(nExpectedSize); - vector vpEdgeKF; + vector> vpEdgeKF; vpEdgeKF.reserve(nExpectedSize); vector vSigmas2; vSigmas2.reserve(nExpectedSize); - vector vpMapPointEdge; + vector> vpMapPointEdge; vpMapPointEdge.reserve(nExpectedSize); const float thHuber = sqrt(5.991); - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { - MapPoint* pMP = *lit; + std::shared_ptr pMP = *lit; g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos())); int id = pMP->mnId+maxKFid+1; @@ -405,12 +405,12 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) vPoint->setMarginalized(true); optimizer.addVertex(vPoint); - map observations = pMP->GetObservations(); + map,size_t> observations = pMP->GetObservations(); //SET EDGES - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for(map,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { - KeyFrame* pKFi = mit->first; + std::shared_ptr pKFi = mit->first; if(!pKFi->isBad()) { @@ -452,14 +452,14 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) for(size_t i=0, iend=vpEdges.size(); i pMP = vpMapPointEdge[i]; if(pMP->isBad()) continue; if(e->chi2()>5.991 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKF[i]; + std::shared_ptr pKFi = vpEdgeKF[i]; pKFi->EraseMapPointMatch(pMP); pMP->EraseObservation(pKFi); @@ -471,17 +471,17 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) // Recover optimized data //Keyframes - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) { - KeyFrame* pKF = *lit; + std::shared_ptr pKF = *lit; g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); pKF->SetPose(Converter::toCvMat(SE3quat)); } //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { - MapPoint* pMP = *lit; + std::shared_ptr pMP = *lit; g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate())); pMP->UpdateNormalAndDepth(); @@ -500,14 +500,14 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) if(!e) continue; - MapPoint* pMP = vpMapPointEdge[i]; + std::shared_ptr pMP = vpMapPointEdge[i]; if(pMP->isBad()) continue; if(e->chi2()>5.991 || !e->isDepthPositive()) { - KeyFrame* pKF = vpEdgeKF[i]; + std::shared_ptr pKF = vpEdgeKF[i]; pKF->EraseMapPointMatch(pMP->GetIndexInKeyFrame(pKF)); pMP->EraseObservation(pKF); } @@ -516,18 +516,18 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) // Recover optimized data //Keyframes - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) { - KeyFrame* pKF = *lit; + std::shared_ptr pKF = *lit; g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); pKF->SetPose(Converter::toCvMat(SE3quat)); } //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for(list>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { - MapPoint* pMP = *lit; + std::shared_ptr pMP = *lit; g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate())); pMP->UpdateNormalAndDepth(); @@ -536,10 +536,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag) -void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, g2o::Sim3 &Scurw, +void Optimizer::OptimizeEssentialGraph(Map* pMap, std::shared_ptr pLoopKF, std::shared_ptr pCurKF, g2o::Sim3 &Scurw, LoopClosing::KeyFrameAndPose &NonCorrectedSim3, LoopClosing::KeyFrameAndPose &CorrectedSim3, - map > &LoopConnections) + map, set> > &LoopConnections) { // Setup optimizer g2o::SparseOptimizer optimizer; @@ -552,8 +552,8 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p solver->setUserLambdaInit(1e-16); optimizer.setAlgorithm(solver); - vector vpKFs = pMap->GetAllKeyFrames(); - vector vpMPs = pMap->GetAllMapPoints(); + vector> vpKFs = pMap->GetAllKeyFrames(); + vector> vpMPs = pMap->GetAllMapPoints(); unsigned int nMaxKFid = pMap->GetMaxKFid(); @@ -567,7 +567,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p // SET KEYFRAME VERTICES for(size_t i=0, iend=vpKFs.size(); i pKF = vpKFs[i]; if(pKF->isBad()) continue; g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); @@ -605,15 +605,15 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p Eigen::Matrix matLambda = Eigen::Matrix::Identity(); // SET LOOP EDGES - for(map >::iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++) + for(map, set> >::iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++) { - KeyFrame* pKF = mit->first; + std::shared_ptr pKF = mit->first; const long unsigned int nIDi = pKF->mnId; - set &spConnections = mit->second; + set> &spConnections = mit->second; g2o::Sim3 Siw = vScw[nIDi]; g2o::Sim3 Swi = Siw.inverse(); - for(set::iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++) + for(set>::iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++) { const long unsigned int nIDj = (*sit)->mnId; if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit) pKF = vpKFs[i]; const int nIDi = pKF->mnId; @@ -648,7 +648,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p else Swi = vScw[nIDi].inverse(); - KeyFrame* pParentKF = pKF->GetParent(); + std::shared_ptr pParentKF = pKF->GetParent(); // Spanning tree edge if(pParentKF) @@ -674,10 +674,10 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p } // Loop edges - set sLoopEdges = pKF->GetLoopEdges(); - for(set::iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++) + set> sLoopEdges = pKF->GetLoopEdges(); + for(set>::iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++) { - KeyFrame* pLKF = *sit; + std::shared_ptr pLKF = *sit; if(pLKF->mnIdmnId) { g2o::Sim3 Slw; @@ -697,10 +697,10 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p } // Covisibility graph edges - vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); - for(vector::iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) + vector> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); + for(vector>::iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) { - KeyFrame* pKFn = *vit; + std::shared_ptr pKFn = *vit; if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) { if(!pKFn->isBad() && pKFn->mnIdmnId) @@ -735,7 +735,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] for(size_t i=0;i pKFi = vpKFs[i]; const int nIDi = pKFi->mnId; @@ -756,7 +756,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose for(size_t i=0, iend=vpMPs.size(); i pMP = vpMPs[i]; if(pMP->isBad()) continue; @@ -768,7 +768,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p } else { - KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); + std::shared_ptr pRefKF = pMP->GetReferenceKeyFrame(); nIDr = pRefKF->mnId; } @@ -787,7 +787,7 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p } } -int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, float th2) +int Optimizer::OptimizeSim3(std::shared_ptrpKF1, std::shared_ptrpKF2, vector> &vpMatches1, g2o::Sim3 &g2oS12, float th2) { g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType * linearSolver; @@ -826,7 +826,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & // SET MAP POINT VERTICES const int N = vpMatches1.size(); - vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector> vpMapPoints1 = pKF1->GetMapPointMatches(); vector vpEdges12; vector vpEdges21; vector vSigmas12, vSigmas21; @@ -845,8 +845,8 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & if(!vpMatches1[i]) continue; - MapPoint* pMP1 = vpMapPoints1[i]; - MapPoint* pMP2 = vpMatches1[i]; + std::shared_ptr pMP1 = vpMapPoints1[i]; + std::shared_ptr pMP2 = vpMatches1[i]; int id1 = 2*i+1; int id2 = 2*(i+1); diff --git a/orb_slam/src/PnPsolver.cc b/orb_slam/src/PnPsolver.cc index 9ee2a73e..c22b705b 100644 --- a/orb_slam/src/PnPsolver.cc +++ b/orb_slam/src/PnPsolver.cc @@ -1,6 +1,6 @@ /** * This file is part of ORB-SLAM. -* This is a modified version of EPnP including a RANSAC scheme +* This is a modified version of EPnP , see FreeBSD license below. * * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) * For more information see @@ -19,6 +19,35 @@ * along with ORB-SLAM. If not, see . */ +/** +* Copyright (c) 2009, V. Lepetit, EPFL +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright notice, this +* list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* The views and conclusions contained in the software and documentation are those +* of the authors and should not be interpreted as representing official policies, +* either expressed or implied, of the FreeBSD Project +*/ + #include #include "PnPsolver.h" @@ -36,7 +65,7 @@ namespace ORB_SLAM { -PnPsolver::PnPsolver(const Frame &F, const vector &vpMapPointMatches): +PnPsolver::PnPsolver(const Frame &F, const vector> &vpMapPointMatches): pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0) { @@ -50,7 +79,7 @@ PnPsolver::PnPsolver(const Frame &F, const vector &vpMapPointMatches) int idx=0; for(size_t i=0, iend=vpMapPointMatches.size(); i pMP = vpMapPointMatches[i]; if(pMP) { @@ -65,7 +94,7 @@ PnPsolver::PnPsolver(const Frame &F, const vector &vpMapPointMatches) mvP3Dw.push_back(cv::Point3f(Pos.at(0),Pos.at(1), Pos.at(2))); mvKeyPointIndices.push_back(i); - mvAllIndices.push_back(idx); + mvAllIndices.push_back(idx); idx++; } @@ -131,7 +160,7 @@ void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxI cv::Mat PnPsolver::find(vector &vbInliers, int &nInliers) { bool bFlag; - return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers); + return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers); } cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers) diff --git a/orb_slam/src/Sim3Solver.cc b/orb_slam/src/Sim3Solver.cc index 7928540a..2c0086ba 100644 --- a/orb_slam/src/Sim3Solver.cc +++ b/orb_slam/src/Sim3Solver.cc @@ -34,13 +34,13 @@ namespace ORB_SLAM { -Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12): +Sim3Solver::Sim3Solver(std::shared_ptrpKF1, std::shared_ptrpKF2, const vector> &vpMatched12): mnIterations(0), mnBestInliers(0) { mpKF1 = pKF1; mpKF2 = pKF2; - vector vpKeyFrameMP1 = pKF1->GetMapPointMatches(); + vector> vpKeyFrameMP1 = pKF1->GetMapPointMatches(); mN1 = vpMatched12.size(); @@ -63,8 +63,8 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector { if(vpMatched12[i1]) { - MapPoint* pMP1 = vpKeyFrameMP1[i1]; - MapPoint* pMP2 = vpMatched12[i1]; + std::shared_ptr pMP1 = vpKeyFrameMP1[i1]; + std::shared_ptr pMP2 = vpMatched12[i1]; if(!pMP1) continue; @@ -115,7 +115,7 @@ void Sim3Solver::SetRansacParameters(double probability, int minInliers, int max { mRansacProb = probability; mRansacMinInliers = minInliers; - mRansacMaxIts = maxIterations; + mRansacMaxIts = maxIterations; N = mvpMapPoints1.size(); // number of correspondences diff --git a/orb_slam/src/StatePublisher.cc b/orb_slam/src/StatePublisher.cc new file mode 100644 index 00000000..93115da8 --- /dev/null +++ b/orb_slam/src/StatePublisher.cc @@ -0,0 +1,49 @@ +/** +* This file is part of ORB-SLAM. +* +* Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM. If not, see . +*/ + +#include "StatePublisher.h" + + +namespace ORB_SLAM +{ + +StatePublisher::StatePublisher(Tracking* tracking):nh("~"), mpTracking(tracking) { + + //Configure Publisher + mPublisher = nh.advertise("state", 10); + + mMsgState.header.frame_id = "ORB_SLAM"; + +} + +void StatePublisher::Publish() +{ + //save computation if their is no subscriber + if(mPublisher.getNumSubscribers() > 0 ) + { + mMsgState.state = static_cast(mpTracking->mState); + mMsgState.header.seq++; + mMsgState.header.stamp = ros::Time::now(); + mPublisher.publish(mMsgState); + } +} + + +} //namespace ORB_SLAM diff --git a/orb_slam/src/Tracking.cc b/orb_slam/src/Tracking.cc index 0b0579cc..61fd4e2e 100644 --- a/orb_slam/src/Tracking.cc +++ b/orb_slam/src/Tracking.cc @@ -19,35 +19,25 @@ */ #include "Tracking.h" -#include -#include +#include #include -#include +#include -#include -#include -#include +#include"ORBmatcher.h" +#include"FramePublisher.h" +#include"Converter.h" +#include"Map.h" +#include"Initializer.h" -#include +#include"Optimizer.h" +#include"PnPsolver.h" -#include +#include +#include -#include "ORBmatcher.h" -#include "FramePublisher.h" -#include "Converter.h" -#include "Map.h" -#include "Initializer.h" - -#include "Optimizer.h" -#include "PnPsolver.h" - -#include -#include using namespace std; -using namespace boost; -namespace fs=filesystem; namespace ORB_SLAM { @@ -55,12 +45,29 @@ namespace ORB_SLAM Tracking::Tracking(ORBVocabulary* pVoc, FramePublisher *pFramePublisher, MapPublisher *pMapPublisher, Map *pMap, string strSettingPath): mState(NO_IMAGES_YET), mpORBVocabulary(pVoc), mpFramePublisher(pFramePublisher), mpMapPublisher(pMapPublisher), mpMap(pMap), - mnLastRelocFrameId(0), mbPublisherStopped(false), mbReseting(false), mbForceRelocalisation(false), mbMotionModel(false), mRange(-1.0), - mRangeStamp(-1.0) + mnLastRelocFrameId(0), mbPublisherStopped(false), mbReseting(false), mbForceRelocalisation(false), mbMotionModel(false) { // Load camera parameters from settings file cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); + float fx = fSettings["Camera.fx"]; + float fy = fSettings["Camera.fy"]; + float cx = fSettings["Camera.cx"]; + float cy = fSettings["Camera.cy"]; + + 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.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"]; + DistCoef.copyTo(mDistCoef); float fps = fSettings["Camera.fps"]; if(fps==0) @@ -70,39 +77,38 @@ Tracking::Tracking(ORBVocabulary* pVoc, FramePublisher *pFramePublisher, MapPubl mMinFrames = 0; mMaxFrames = 18*fps/30; - int nRGB = fSettings["Camera.RGB"]; - mbRGB = nRGB; cout << "Camera Parameters: " << endl; + cout << "- fx: " << fx << endl; + cout << "- fy: " << fy << endl; + cout << "- cx: " << cx << endl; + cout << "- cy: " << cy << endl; + cout << "- k1: " << DistCoef.at(0) << endl; + cout << "- k2: " << DistCoef.at(1) << endl; + cout << "- p1: " << DistCoef.at(2) << endl; + cout << "- p2: " << DistCoef.at(3) << endl; cout << "- fps: " << fps << endl; + + + int nRGB = fSettings["Camera.RGB"]; + mbRGB = nRGB; + if(mbRGB) cout << "- color order: RGB (ignored if grayscale)" << endl; else cout << "- color order: BGR (ignored if grayscale)" << endl; - // Are the images rectified? - int rectified = fSettings["Camera.Rectified"]; - if (rectified != 0 ) - { - mRectified = true; - cout << "- Images are rectified: Yes" << endl; - } - else - { - mRectified = false; - cout << "- Images are rectified: No" << endl; - } - // Load ORB parameters + int nFeatures = fSettings["ORBextractor.nFeatures"]; float fScaleFactor = fSettings["ORBextractor.scaleFactor"]; int nLevels = fSettings["ORBextractor.nLevels"]; - int fastTh = fSettings["ORBextractor.fastTh"]; + int fastTh = fSettings["ORBextractor.fastTh"]; int Score = fSettings["ORBextractor.nScoreType"]; assert(Score==1 || Score==0); - mpORBextractor = new ORBextractor(nFeatures,fScaleFactor,nLevels,Score,fastTh); + mpORBextractor = std::shared_ptr(new ORBextractor(nFeatures,fScaleFactor,nLevels,Score,fastTh)); cout << endl << "ORB Extractor Parameters: " << endl; cout << "- Number of Features: " << nFeatures << endl; @@ -117,7 +123,7 @@ Tracking::Tracking(ORBVocabulary* pVoc, FramePublisher *pFramePublisher, MapPubl // ORB extractor for initialization // Initialization uses only points from the finest scale level - mpIniORBextractor = new ORBextractor(nFeatures*2,1.2,8,Score,fastTh); + mpIniORBextractor = std::shared_ptr(new ORBextractor(nFeatures*2,1.2,8,Score,fastTh)); int nMotion = fSettings["UseMotionModel"]; mbMotionModel = nMotion; @@ -154,34 +160,15 @@ void Tracking::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB) void Tracking::Run() { ros::NodeHandle nodeHandler; - image_transport::ImageTransport it(nodeHandler); - - mImageSub.subscribe(it, "/camera/image_raw", 5); - mInfoSub.subscribe(nodeHandler, "/camera/camera_info", 5); - mSyncImages.reset(new mPoliceSyncImages(mPoliceSyncImages(5), mImageSub, mInfoSub) ); - mSyncImages->registerCallback(bind(&Tracking::GrabImages, this, _1, _2)); - - mImageSub2.subscribe(it, "/camera/image_raw", 20); - mRangeSub.subscribe(nodeHandler, "/camera/range", 20); - mSyncRange.reset(new mPoliceSyncRange(mPoliceSyncRange(20), mImageSub2, mRangeSub) ); - mSyncRange->registerCallback(bind(&Tracking::GrabRange, this, _1, _2)); + ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &Tracking::GrabImage, this); ros::spin(); } -void Tracking::GrabRange(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::RangeConstPtr& range) +void Tracking::GrabImage(const sensor_msgs::ImageConstPtr& msg) { - boost::mutex::scoped_lock lock(mMutexRange); - mRange = range->range; - mRangeStamp = msg->header.stamp.toSec(); -} -void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, - const sensor_msgs::CameraInfoConstPtr& info) -{ cv::Mat im; - ros::Time stamp = msg->header.stamp; // Copy the ros image message to cv::Mat. Convert to grayscale if it is a color image. cv_bridge::CvImageConstPtr cv_ptr; @@ -195,9 +182,9 @@ void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, return; } - ROS_ASSERT(cv_ptr->image.channels()==3 || cv_ptr->image.channels()==1); + ROS_ASSERT(cv_ptr->image.channels()>=3 || cv_ptr->image.channels()==1); - if(cv_ptr->image.channels()==3) + if(cv_ptr->image.channels()>=3) { if(mbRGB) cvtColor(cv_ptr->image, im, CV_RGB2GRAY); @@ -209,31 +196,18 @@ void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, cv_ptr->image.copyTo(im); } + if(mState==WORKING || mState==LOST) + mCurrentFrame = Frame(im,cv_ptr->header.stamp.toSec(),mpORBextractor,mpORBVocabulary,mK,mDistCoef); + else + mCurrentFrame = Frame(im,cv_ptr->header.stamp.toSec(),mpIniORBextractor,mpORBVocabulary,mK,mDistCoef); + // Depending on the state of the Tracker we perform different tasks + if(mState==NO_IMAGES_YET) { - // Read the camera parameters from CameraInfo message - image_geometry::PinholeCameraModel pinholeCameraModel; - pinholeCameraModel.fromCameraInfo(info); - cv::Mat K = cv::Mat::eye(3,3,CV_32F); - K.at(0,0) = pinholeCameraModel.fx() / pinholeCameraModel.binningX(); - K.at(1,1) = pinholeCameraModel.fy() / pinholeCameraModel.binningY(); - K.at(0,2) = pinholeCameraModel.cx() / pinholeCameraModel.binningX(); - K.at(1,2) = pinholeCameraModel.cy() / pinholeCameraModel.binningY(); - K.copyTo(mK); - cv::Mat distCoef = cv::Mat::zeros(1,1,CV_32F); - if (!mRectified) - distCoef = pinholeCameraModel.distortionCoeffs().t(); - distCoef.copyTo(mDistCoef); - mState = NOT_INITIALIZED; } - if(mState==WORKING || mState==LOST) - mCurrentFrame = Frame(im,stamp.toSec(),mpORBextractor,mpORBVocabulary,mK,mDistCoef); - else - mCurrentFrame = Frame(im,stamp.toSec(),mpIniORBextractor,mpORBVocabulary,mK,mDistCoef); - mLastProcessedState=mState; if(mState==NOT_INITIALIZED) @@ -253,9 +227,7 @@ void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, if(mState==WORKING && !RelocalisationRequested()) { if(!mbMotionModel || mpMap->KeyFramesInMap()<4 || mVelocity.empty() || mCurrentFrame.mnIdSetCurrentCameraPose(mCurrentFrame.mTcw, mCurrentFrame.mTimeStamp, mRange, mRangeStamp); + mpMapPublisher->SetCurrentCameraPose(mCurrentFrame.mTcw); if(NeedNewKeyFrame()) CreateNewKeyFrame(); @@ -323,7 +295,7 @@ void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, } mLastFrame = Frame(mCurrentFrame); - } + } // Update drawer mpFramePublisher->Update(this); @@ -341,8 +313,10 @@ void Tracking::GrabImages(const sensor_msgs::ImageConstPtr& msg, mTfBr.sendTransform(tf::StampedTransform(tfTcw,ros::Time::now(), "ORB_SLAM/World", "ORB_SLAM/Camera")); } + } + void Tracking::FirstInitialization() { //We ensure a minimum ORB features to continue, otherwise discard frame @@ -354,10 +328,7 @@ void Tracking::FirstInitialization() for(size_t i=0; i(new Initializer(mCurrentFrame,1.0,200)); mState = INITIALIZING; @@ -372,7 +343,7 @@ void Tracking::Initialize() fill(mvIniMatches.begin(),mvIniMatches.end(),-1); mState = NOT_INITIALIZED; return; - } + } // Find correspondences ORBmatcher matcher(0.9,true); @@ -383,7 +354,7 @@ void Tracking::Initialize() { mState = NOT_INITIALIZED; return; - } + } cv::Mat Rcw; // Current Camera Rotation cv::Mat tcw; // Current Camera Translation @@ -397,7 +368,7 @@ void Tracking::Initialize() { mvIniMatches[i]=-1; nmatches--; - } + } } CreateInitialMap(Rcw,tcw); @@ -414,8 +385,8 @@ void Tracking::CreateInitialMap(cv::Mat &Rcw, cv::Mat &tcw) tcw.copyTo(mCurrentFrame.mTcw.rowRange(0,3).col(3)); // Create KeyFrames - KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB); - KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + std::shared_ptr pKFini( new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB)); + std::shared_ptr pKFcur( new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB)); pKFini->ComputeBoW(); pKFcur->ComputeBoW(); @@ -433,7 +404,7 @@ void Tracking::CreateInitialMap(cv::Mat &Rcw, cv::Mat &tcw) //Create MapPoint. cv::Mat worldPos(mvIniP3D[i]); - MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap); + std::shared_ptr pMP(new MapPoint(worldPos,pKFcur,mpMap)); pKFini->AddMapPoint(pMP,i); pKFcur->AddMapPoint(pMP,mvIniMatches[i]); @@ -478,12 +449,12 @@ void Tracking::CreateInitialMap(cv::Mat &Rcw, cv::Mat &tcw) pKFcur->SetPose(Tc2w); // Scale points - vector vpAllMapPoints = pKFini->GetMapPointMatches(); + vector> vpAllMapPoints = pKFini->GetMapPointMatches(); for(size_t iMP=0; iMP pMP = vpAllMapPoints[iMP]; pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); } } @@ -503,15 +474,16 @@ void Tracking::CreateInitialMap(cv::Mat &Rcw, cv::Mat &tcw) mpMap->SetReferenceMapPoints(mvpLocalMapPoints); - mpMapPublisher->SetCurrentCameraPose(pKFcur->GetPose(), pKFcur->mTimeStamp, mRange, mRangeStamp); + mpMapPublisher->SetCurrentCameraPose(pKFcur->GetPose()); mState=WORKING; } + bool Tracking::TrackPreviousFrame() { ORBmatcher matcher(0.9,true); - vector vpMapPointMatches; + vector> vpMapPointMatches; // Search first points at coarse scale levels to get a rough initial estimate int minOctave = 0; @@ -527,7 +499,7 @@ bool Tracking::TrackPreviousFrame() nmatches = matcher.WindowSearch(mLastFrame,mCurrentFrame,100,vpMapPointMatches,0); if(nmatches<10) { - vpMapPointMatches=vector(mCurrentFrame.mvpMapPoints.size(),static_cast(NULL)); + vpMapPointMatches=vector>(mCurrentFrame.mvpMapPoints.size(),static_cast>(NULL)); nmatches=0; } } @@ -555,6 +527,7 @@ bool Tracking::TrackPreviousFrame() else //Last opportunity nmatches = matcher.SearchByProjection(mLastFrame,mCurrentFrame,50,vpMapPointMatches); + mCurrentFrame.mvpMapPoints=vpMapPointMatches; if(nmatches<10) @@ -578,12 +551,12 @@ bool Tracking::TrackPreviousFrame() bool Tracking::TrackWithMotionModel() { ORBmatcher matcher(0.9,true); - vector vpMapPointMatches; + vector> vpMapPointMatches; // Compute current pose by motion model mCurrentFrame.mTcw = mVelocity*mLastFrame.mTcw; - fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast>(NULL)); // Project points seen in previous frame int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,15); @@ -688,8 +661,10 @@ bool Tracking::NeedNewKeyFrame() void Tracking::CreateNewKeyFrame() { - KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + std::shared_ptr pKF(new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB)); + mpLocalMapper->InsertKeyFrame(pKF); + mnLastKeyFrameId = mCurrentFrame.mnId; mpLastKeyFrame = pKF; } @@ -697,9 +672,9 @@ void Tracking::CreateNewKeyFrame() void Tracking::SearchReferencePointsInFrustum() { // Do not search map points already matched - for(vector::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) + for(vector>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) { - MapPoint* pMP = *vit; + std::shared_ptr pMP = *vit; if(pMP) { if(pMP->isBad()) @@ -720,20 +695,20 @@ void Tracking::SearchReferencePointsInFrustum() int nToMatch=0; // Project points in frame and check its visibility - for(vector::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) + for(vector>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) { - MapPoint* pMP = *vit; + std::shared_ptr pMP = *vit; if(pMP->mnLastFrameSeen == mCurrentFrame.mnId) continue; if(pMP->isBad()) - continue; + continue; // Project (this fills MapPoint variables for matching) if(mCurrentFrame.isInFrustum(pMP,0.5)) { pMP->IncreaseVisible(); nToMatch++; } - } + } if(nToMatch>0) @@ -748,7 +723,7 @@ void Tracking::SearchReferencePointsInFrustum() } void Tracking::UpdateReference() -{ +{ // This is for visualization mpMap->SetReferenceMapPoints(mvpLocalMapPoints); @@ -761,14 +736,14 @@ void Tracking::UpdateReferencePoints() { mvpLocalMapPoints.clear(); - for(vector::iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) + for(vector>::iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) { - KeyFrame* pKF = *itKF; - vector vpMPs = pKF->GetMapPointMatches(); + std::shared_ptr pKF = *itKF; + vector> vpMPs = pKF->GetMapPointMatches(); - for(vector::iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++) + for(vector>::iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++) { - MapPoint* pMP = *itMP; + std::shared_ptr pMP = *itMP; if(!pMP) continue; if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId) @@ -786,16 +761,16 @@ void Tracking::UpdateReferencePoints() void Tracking::UpdateReferenceKeyFrames() { // Each map point vote for the keyframes in which it has been observed - map keyframeCounter; + map,int> keyframeCounter; for(size_t i=0, iend=mCurrentFrame.mvpMapPoints.size(); i pMP = mCurrentFrame.mvpMapPoints[i]; if(!pMP->isBad()) { - map observations = pMP->GetObservations(); - for(map::iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) + map,size_t> observations = pMP->GetObservations(); + for(map,size_t>::iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) keyframeCounter[it->first]++; } else @@ -806,15 +781,15 @@ void Tracking::UpdateReferenceKeyFrames() } int max=0; - KeyFrame* pKFmax=NULL; + std::shared_ptr pKFmax=NULL; mvpLocalKeyFrames.clear(); mvpLocalKeyFrames.reserve(3*keyframeCounter.size()); // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points - for(map::iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) + for(map,int>::iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) { - KeyFrame* pKF = it->first; + std::shared_ptr pKF = it->first; if(pKF->isBad()) continue; @@ -831,19 +806,19 @@ void Tracking::UpdateReferenceKeyFrames() // Include also some not-already-included keyframes that are neighbors to already-included keyframes - for(vector::iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) + for(vector>::iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) { // Limit the number of keyframes if(mvpLocalKeyFrames.size()>80) break; - KeyFrame* pKF = *itKF; + std::shared_ptr pKF = *itKF; - vector vNeighs = pKF->GetBestCovisibilityKeyFrames(10); + vector> vNeighs = pKF->GetBestCovisibilityKeyFrames(10); - for(vector::iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++) + for(vector>::iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++) { - KeyFrame* pNeighKF = *itNeighKF; + std::shared_ptr pNeighKF = *itNeighKF; if(!pNeighKF->isBad()) { if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId) @@ -867,7 +842,7 @@ bool Tracking::Relocalisation() // Relocalisation is performed when tracking is lost and forced at some stages during loop closing // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation - vector vpCandidateKFs; + vector> vpCandidateKFs; if(!RelocalisationRequested()) vpCandidateKFs= mpKeyFrameDB->DetectRelocalisationCandidates(&mCurrentFrame); else // Forced Relocalisation: Relocate against local window around last keyframe @@ -888,10 +863,10 @@ bool Tracking::Relocalisation() // If enough matches are found we setup a PnP solver ORBmatcher matcher(0.75,true); - vector vpPnPsolvers; + vector> vpPnPsolvers; vpPnPsolvers.resize(nKFs); - vector > vvpMapPointMatches; + vector> > vvpMapPointMatches; vvpMapPointMatches.resize(nKFs); vector vbDiscarded; @@ -901,7 +876,7 @@ bool Tracking::Relocalisation() for(size_t i=0; i pKF = vpCandidateKFs[i]; if(pKF->isBad()) vbDiscarded[i] = true; else @@ -914,12 +889,12 @@ bool Tracking::Relocalisation() } else { - PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); + shared_ptr pSolver(new PnPsolver(mCurrentFrame,vvpMapPointMatches[i])); pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); vpPnPsolvers[i] = pSolver; nCandidates++; } - } + } } // Alternatively perform some iterations of P4P RANSAC @@ -939,7 +914,7 @@ bool Tracking::Relocalisation() int nInliers; bool bNoMore; - PnPsolver* pSolver = vpPnPsolvers[i]; + shared_ptr pSolver = vpPnPsolvers[i]; cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); // If Ransac reachs max. iterations discard keyframe @@ -954,7 +929,7 @@ bool Tracking::Relocalisation() { Tcw.copyTo(mCurrentFrame.mTcw); - set sFound; + set> sFound; for(size_t j=0; j=50) - { + { bMatch = true; break; } diff --git a/orb_slam/src/main.cc b/orb_slam/src/main.cc index caae961a..bd1f409a 100644 --- a/orb_slam/src/main.cc +++ b/orb_slam/src/main.cc @@ -18,22 +18,24 @@ * along with ORB-SLAM. If not, see . */ -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include #include "Tracking.h" #include "FramePublisher.h" #include "Map.h" #include "MapPublisher.h" +#include "OctoMapPublisher.h" #include "LocalMapping.h" #include "LoopClosing.h" #include "KeyFrameDatabase.h" #include "ORBVocabulary.h" +#include "StatePublisher.h" #include "Converter.h" @@ -44,7 +46,7 @@ using namespace std; int main(int argc, char **argv) { - ros::init(argc, argv, "ORB_SLAM"); + ros::init(argc, argv, "orb_slam"); ros::start(); cout << endl << "ORB-SLAM Copyright (C) 2014 Raul Mur-Artal" << endl << @@ -54,7 +56,7 @@ int main(int argc, char **argv) if(argc != 3) { - cerr << endl << "Usage: rosrun ORB_SLAM ORB_SLAM path_to_vocabulary path_to_settings (absolute or relative to package directory)" << endl; + cerr << endl << "Usage: rosrun orb_slam orb_slam path_to_vocabulary path_to_settings (absolute or relative to package directory)" << endl; ros::shutdown(); return 1; } @@ -65,7 +67,7 @@ int main(int argc, char **argv) cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); if(!fsSettings.isOpened()) { - ROS_ERROR("Wrong path to settings. Path must be absolut or relative to ORB_SLAM package directory."); + ROS_ERROR("Wrong path to settings. Path must be absolut or relative to orb_slam package directory."); ros::shutdown(); return 1; } @@ -74,17 +76,36 @@ int main(int argc, char **argv) ORB_SLAM::FramePublisher FramePub; //Load ORB Vocabulary + /* Old version to load vocabulary using cv::FileStorage string strVocFile = ros::package::getPath("orb_slam")+"/"+argv[1]; cout << endl << "Loading ORB Vocabulary. This could take a while." << endl; cv::FileStorage fsVoc(strVocFile.c_str(), cv::FileStorage::READ); if(!fsVoc.isOpened()) { - cerr << endl << "Wrong path to vocabulary. Path must be absolut or relative to ORB_SLAM package directory." << endl; + cerr << endl << "Wrong path to vocabulary. Path must be absolut or relative to orb_slam package directory." << endl; ros::shutdown(); return 1; } ORB_SLAM::ORBVocabulary Vocabulary; Vocabulary.load(fsVoc); + */ + + // New version to load vocabulary from text file "Data/ORBvoc.txt". + // If you have an own .yml vocabulary, use the function + // saveToTextFile in Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h + string strVocFile = ros::package::getPath("orb_slam")+"/"+argv[1]; + cout << endl << "Loading ORB Vocabulary. This could take a while." << endl; + + ORB_SLAM::ORBVocabulary Vocabulary; + bool bVocLoad = Vocabulary.loadFromTextFile(strVocFile); + + if(!bVocLoad) + { + cerr << "Wrong path to vocabulary. Path must be absolut or relative to orb_slam package directory." << endl; + cerr << "Falied to open at: " << strVocFile << endl; + ros::shutdown(); + return 1; + } cout << "Vocabulary loaded!" << endl << endl; @@ -99,6 +120,9 @@ int main(int argc, char **argv) //Create Map Publisher for Rviz ORB_SLAM::MapPublisher MapPub(&World); + //Create a OctoMap service provider and publisher + ORB_SLAM::OctoMapPublisher OctoMapServPub(&World); + //Initialize the Tracking Thread and launch ORB_SLAM::Tracking Tracker(&Vocabulary, &FramePub, &MapPub, &World, strSettingsFile); boost::thread trackingThread(&ORB_SLAM::Tracking::Run,&Tracker); @@ -109,7 +133,7 @@ int main(int argc, char **argv) ORB_SLAM::LocalMapping LocalMapper(&World); boost::thread localMappingThread(&ORB_SLAM::LocalMapping::Run,&LocalMapper); - // //Initialize the Loop Closing Thread and launch + //Initialize the Loop Closing Thread and launch ORB_SLAM::LoopClosing LoopCloser(&World, &Database, &Vocabulary); boost::thread loopClosingThread(&ORB_SLAM::LoopClosing::Run, &LoopCloser); @@ -123,6 +147,8 @@ int main(int argc, char **argv) LoopCloser.SetTracker(&Tracker); LoopCloser.SetLocalMapper(&LocalMapper); + ORB_SLAM::StatePublisher StatePublisher(&Tracker); + //This "main" thread will show the current processed frame and publish the map float fps = fsSettings["Camera.fps"]; if(fps==0) @@ -135,26 +161,25 @@ int main(int argc, char **argv) FramePub.Refresh(); MapPub.Refresh(); Tracker.CheckResetByPublishers(); + OctoMapServPub.Publish(); + StatePublisher.Publish(); r.sleep(); } // Save keyframe poses at the end of the execution ofstream f; - vector vpKFs = World.GetAllKeyFrames(); + vector> vpKFs = World.GetAllKeyFrames(); sort(vpKFs.begin(),vpKFs.end(),ORB_SLAM::KeyFrame::lId); cout << endl << "Saving Keyframe Trajectory to KeyFrameTrajectory.txt" << endl; string strFile = ros::package::getPath("orb_slam")+"/"+"KeyFrameTrajectory.txt"; f.open(strFile.c_str()); f << fixed; - float rangeScale = MapPub.GetRangeScale(); - if (rangeScale < 0) - rangeScale = 1; for(size_t i=0; i pKF = vpKFs[i]; if(pKF->isBad()) continue; @@ -162,8 +187,8 @@ int main(int argc, char **argv) cv::Mat R = pKF->GetRotation().t(); vector q = ORB_SLAM::Converter::toQuaternion(R); cv::Mat t = pKF->GetCameraCenter(); - f << setprecision(6) << pKF->mTimeStamp << "," << pKF->mnId << setprecision(7) << "," << t.at(0)*rangeScale << "," << t.at(1)*rangeScale << "," << t.at(2)*rangeScale - << "," << q[0] << "," << q[1] << "," << q[2] << "," << q[3] << endl; + f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at(0) << " " << t.at(1) << " " << t.at(2) + << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; } f.close(); diff --git a/orb_slam/srv/SaveOctomap.srv b/orb_slam/srv/SaveOctomap.srv new file mode 100644 index 00000000..81111797 --- /dev/null +++ b/orb_slam/srv/SaveOctomap.srv @@ -0,0 +1,2 @@ +string filename +--- \ No newline at end of file