From feedb9bdd89d6820be25ad3391e27657153775ce Mon Sep 17 00:00:00 2001 From: electech6 Date: Mon, 9 Aug 2021 19:34:51 +0800 Subject: [PATCH] merge the newest code from author --- .gitignore | 6 +- CMakeLists.txt | 40 +- Changelog.md | 21 +- .../Monocular-Inertial/mono_inertial_euroc.cc | 25 +- .../mono_inertial_tum_vi.cc | 1 - Examples/Monocular/mono_euroc.cc | 12 +- Examples/Monocular/mono_kitti.cc | 4 - Examples/Monocular/mono_tum.cc | 4 - Examples/Monocular/mono_tum_vi.cc | 8 +- Examples/RGB-D/rgbd_tum.cc | 8 +- .../Stereo-Inertial/stereo_inertial_euroc.cc | 16 +- .../Stereo-Inertial/stereo_inertial_tum_vi.cc | 16 +- Examples/Stereo/stereo_euroc.cc | 19 +- Examples/Stereo/stereo_kitti.cc | 6 +- Examples/Stereo/stereo_tum_vi.cc | 5 +- README.md | 9 +- Thirdparty/DBoW2/CMakeLists.txt | 6 +- Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h | 51 +- build.sh | 4 +- include/Atlas.h | 34 - include/CameraModels/GeometricCamera.h | 15 +- include/CameraModels/KannalaBrandt8.h | 16 +- include/CameraModels/Pinhole.h | 14 +- include/Frame.h | 16 +- include/KeyFrame.h | 264 +----- include/KeyFrameDatabase.h | 12 - include/LocalMapping.h | 26 +- include/LoopClosing.h | 26 +- include/MLPnPsolver.h | 2 - include/Map.h | 44 +- include/MapPoint.h | 101 +-- include/ORBextractor.h | 2 +- include/ORBmatcher.h | 3 + include/Optimizer.h | 4 +- include/System.h | 19 +- include/Tracking.h | 30 +- src/Atlas.cc | 75 -- src/CameraModels/KannalaBrandt8.cpp | 138 ++- src/CameraModels/Pinhole.cpp | 52 +- src/Frame.cc | 170 ++-- src/FrameDrawer.cc | 91 +- src/G2oTypes.cc | 203 ----- src/ImuTypes.cc | 11 +- src/Initializer.cc | 4 +- src/KeyFrame.cc | 298 +++---- src/KeyFrameDatabase.cc | 30 - src/LocalMapping.cc | 346 ++++---- src/LoopClosing.cc | 469 ++-------- src/Map.cc | 149 ---- src/MapDrawer.cc | 32 - src/MapPoint.cc | 97 +- src/ORBextractor.cc | 2 +- src/ORBmatcher.cc | 256 +++++- src/Optimizer.cc | 828 ++---------------- src/Sim3Solver.cc | 4 +- src/System.cc | 320 +------ src/Tracking.cc | 704 ++++++++++----- src/Viewer.cc | 33 - 58 files changed, 1796 insertions(+), 3405 deletions(-) diff --git a/.gitignore b/.gitignore index f543cc5..c3d1361 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,6 @@ CMakeLists.txt.user CMakeLists_modified.txt -Examples/ + Examples/RGB-D/rgbd_tum Examples/Monocular/mono_euroc @@ -47,7 +47,3 @@ cmake-build-debug/ *.pyc *.osa - -.vscode/ -CMakeLists.txt -build.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 67c356c..3c731bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,16 +4,16 @@ project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) ENDIF() -# set(CMAKE_BUILD_TYPE "Debug") + MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") -# set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") -# set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") +set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native") +# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-deprecated -O3 -march=native ") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-deprecated -O3 -march=native") # Check C++11 or C++0x support include(CheckCXXCompilerFlag) @@ -33,12 +33,12 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 3) +find_package(OpenCV 4.0) if(NOT OpenCV_FOUND) - find_package(OpenCV 2.4.3 QUIET) - if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 2.4.3 not found.") - endif() + find_package(OpenCV 3.0) + if(NOT OpenCV_FOUND) + message(FATAL_ERROR "OpenCV > 3.0 not found.") + endif() endif() MESSAGE("OPENCV VERSION:") @@ -46,6 +46,7 @@ MESSAGE(${OpenCV_VERSION}) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) +find_package(realsense2) include_directories( ${PROJECT_SOURCE_DIR} @@ -72,7 +73,6 @@ src/Atlas.cc src/Map.cc src/MapDrawer.cc src/Optimizer.cc -src/PnPsolver.cc src/Frame.cc src/KeyFrameDatabase.cc src/Sim3Solver.cc @@ -84,6 +84,7 @@ src/CameraModels/Pinhole.cpp src/CameraModels/KannalaBrandt8.cpp src/OptimizableTypes.cpp src/MLPnPsolver.cpp +src/TwoViewReconstruction.cc include/System.h include/Tracking.h include/LocalMapping.h @@ -98,7 +99,6 @@ include/Atlas.h include/Map.h include/MapDrawer.h include/Optimizer.h -include/PnPsolver.h include/Frame.h include/KeyFrameDatabase.h include/Sim3Solver.h @@ -112,7 +112,8 @@ include/CameraModels/KannalaBrandt8.h include/OptimizableTypes.h include/MLPnPsolver.h include/TwoViewReconstruction.h -src/TwoViewReconstruction.cc) +include/Config.h +) add_subdirectory(Thirdparty/g2o) @@ -127,14 +128,17 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so ) -# Build examples +### Build examples +# RGB-D examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) + add_executable(rgbd_tum Examples/RGB-D/rgbd_tum.cc) target_link_libraries(rgbd_tum ${PROJECT_NAME}) +# Stereo examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) add_executable(stereo_kitti @@ -149,7 +153,7 @@ add_executable(stereo_tum_vi Examples/Stereo/stereo_tum_vi.cc) target_link_libraries(stereo_tum_vi ${PROJECT_NAME}) - +# Monocular examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) add_executable(mono_tum @@ -168,7 +172,7 @@ add_executable(mono_tum_vi Examples/Monocular/mono_tum_vi.cc) target_link_libraries(mono_tum_vi ${PROJECT_NAME}) - +# Monocular-Inertial examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial) add_executable(mono_inertial_euroc @@ -179,16 +183,14 @@ add_executable(mono_inertial_tum_vi Examples/Monocular-Inertial/mono_inertial_tum_vi.cc) target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME}) - +# Stereo-Inertial examples set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial) add_executable(stereo_inertial_euroc Examples/Stereo-Inertial/stereo_inertial_euroc.cc) target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME}) - add_executable(stereo_inertial_tum_vi Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc) target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME}) - diff --git a/Changelog.md b/Changelog.md index 1d19d80..ecdffc4 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,15 +1,26 @@ # ORB-SLAM3 Details of changes between the different versions. +### V0.4: Beta version, 21 April 2021 + +- Changed OpenCV dynamic matrices to static matrices to speed up the code. + +- Capability to measure running time of the system threads. + +- Compatibility with OpenCV 4.0 (Requires at least OpenCV 3.0) + +- Fixed minor bugs. + + ### V0.3: Beta version, 4 Sep 2020 -- RGB-D compatibility, the RGB-D examples had been adapted to the new version. +- RGB-D compatibility, the RGB-D examples have been adapted to the new version. -- Kitti and TUM dataset compatibility, these examples had been adapted to the new version. +- Kitti and TUM dataset compatibility, these examples have been adapted to the new version. -- ROS compatibility, It had been updated the old references in the code to work with this version. +- ROS compatibility, updated the old references in the code to work with this version. -- Config file parser, the YAML file contains the session configuration, a wrong parametrization may break the execution without any information to solve it. This version parses the file to read all the fields and give a proper answer if one of the fields have been wrong deffined or don't exist. +- Config file parser, the YAML file contains the session configuration, a wrong parametrization may break the execution without any information to solve it. This version parses the file to read all the fields and give a proper answer if one of the fields have been wrongly deffined or does not exist. - Fixed minor bugs. @@ -19,7 +30,7 @@ Initial release. It has these capabilities: - Multiple-Maps capabilities, it is able to handle multiple maps in the same session and merge them when a common area is detected with a seamless fussion. -- Inertial sensor, the IMU initialization takes a 2 seconds to achieve a scale error less than 5\% and it is reffined in the next 10 seconds until is around 1\%. Inertial measures are integrated at frame rate to estimate the scale, gravity and velocity in order to improve the visual features detection and make the system robust to temporal occlusions. +- Inertial sensor, the IMU initialization takes 2 seconds to achieve a scale error less than 5\% and it is reffined in the next 10 seconds until it is around 1\%. Inertial measures are integrated at frame rate to estimate the scale, gravity and velocity in order to improve the visual features detection and make the system robust to temporal occlusions. - Fisheye sensor, the fisheye sensors are now fully supported in monocular and stereo. diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc index f87a70b..7305208 100644 --- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc @@ -116,25 +116,19 @@ int main(int argc, char *argv[]) cout.precision(17); - /*cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl; - cout << "IMU data in the sequence: " << nImu << endl << endl;*/ - // Create SLAM system. It initializes all system threads and gets ready to process frames. ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true); int proccIm=0; for (seq = 0; seq vImuMeas; proccIm = 0; for(int ni=0; ni0) { - // cout << "t_cam " << tframe << endl; - while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) { vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z, @@ -161,9 +153,7 @@ int main(int argc, char *argv[]) } } - /*cout << "first imu: " << first_imu << endl; - cout << "first imu time: " << fixed << vTimestampsImu[first_imu] << endl; - cout << "size vImu: " << vImuMeas.size() << endl;*/ + #ifdef COMPILEDWITHC11 std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); #else @@ -171,8 +161,7 @@ int main(int argc, char *argv[]) #endif // Pass the image to the SLAM system - // cout << "tframe = " << tframe << endl; - SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial + SLAM.TrackMonocular(im,tframe,vImuMeas); #ifdef COMPILEDWITHC11 std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); @@ -180,9 +169,13 @@ int main(int argc, char *argv[]) std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now(); #endif +#ifdef REGISTER_TIMES + double t_track = std::chrono::duration_cast >(t2 - t1).count(); + SLAM.InsertTrackTime(t_track); +#endif + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); ttrack_tot += ttrack; - // std::cout << "ttrack: " << ttrack << std::endl; vTimesTrack[ni]=ttrack; @@ -194,7 +187,7 @@ int main(int argc, char *argv[]) T = tframe-vTimestampsCam[seq][ni-1]; if(ttrack vTimesTrack; vTimesTrack.resize(nImages); - cout << endl << "-------" << endl; - cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; - // Main loop cv::Mat im; for(int ni=0; niapply(im,im); @@ -146,7 +146,7 @@ int main(int argc, char **argv) T = tframe-vTimestampsCam[seq][ni-1]; if(ttrack vImuMeas; double t_rect = 0; + double t_track = 0; int num_rect = 0; int proccIm = 0; for(int ni=0; ni >(t_End_Rect - t_Start_Rect).count(); + t_rect = std::chrono::duration_cast >(t_End_Rect - t_Start_Rect).count(); + SLAM.InsertRectTime(t_rect); +#endif double tframe = vTimestampsCam[seq][ni]; // Load imu measurements from previous frame vImuMeas.clear(); if(ni>0) - while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni]) + while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) { vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z, vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z, @@ -234,6 +239,11 @@ int main(int argc, char **argv) std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now(); #endif +#ifdef REGISTER_TIMES + t_track = t_rect + std::chrono::duration_cast >(t2 - t1).count(); + SLAM.InsertTrackTime(t_track); +#endif + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); vTimesTrack[ni]=ttrack; diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc index 3741f92..38f1c7a 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc @@ -114,10 +114,6 @@ int main(int argc, char **argv) cout << endl << "-------" << endl; cout.precision(17); - /*cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl; - cout << "IMU data in the sequence: " << nImu << endl << endl;*/ - // Create SLAM system. It initializes all system threads and gets ready to process frames. ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, true, 0, file_name); @@ -156,23 +152,16 @@ int main(int argc, char **argv) if(ni>0) { - // cout << "t_cam " << tframe << endl; while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) { - // vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu])); vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z, vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z, vTimestampsImu[seq][first_imu[seq]])); - // cout << "t_imu = " << fixed << vImuMeas.back().t << endl; first_imu[seq]++; } } - /*cout << "first imu: " << first_imu[seq] << endl; - cout << "first imu time: " << fixed << vTimestampsImu[seq][0] << endl; - cout << "size vImu: " << vImuMeas.size() << endl;*/ - #ifdef COMPILEDWITHC11 std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); #else @@ -190,7 +179,6 @@ int main(int argc, char **argv) double ttrack= std::chrono::duration_cast >(t2 - t1).count(); ttrack_tot += ttrack; - // std::cout << "ttrack: " << ttrack << std::endl; vTimesTrack[ni]=ttrack; @@ -202,7 +190,7 @@ int main(int argc, char **argv) T = tframe-vTimestampsCam[seq][ni-1]; if(ttrack >(t_End_Rect - t_Start_Rect).count(); + t_rect = std::chrono::duration_cast >(t_End_Rect - t_Start_Rect).count(); + SLAM.InsertRectTime(t_rect); +#endif double tframe = vTimestampsCam[seq][ni]; - - #ifdef COMPILEDWITHC11 std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); #else @@ -191,6 +193,11 @@ int main(int argc, char **argv) std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now(); #endif +#ifdef REGISTER_TIMES + t_track = t_rect + std::chrono::duration_cast >(t2 - t1).count(); + SLAM.InsertTrackTime(t_track); +#endif + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); vTimesTrack[ni]=ttrack; @@ -203,7 +210,7 @@ int main(int argc, char **argv) T = tframe-vTimestampsCam[seq][ni-1]; if(ttrack vTimesTrack; - vTimesTrack.resize(nImages); - - cout << endl << "-------" << endl; - cout << "Start processing sequence ..." << endl; - cout << "Images in the sequence: " << nImages << endl << endl; + vTimesTrack.resize(nImages); // Main loop cv::Mat imLeft, imRight; diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc index 8d5d467..553cd55 100644 --- a/Examples/Stereo/stereo_tum_vi.cc +++ b/Examples/Stereo/stereo_tum_vi.cc @@ -137,7 +137,6 @@ int main(int argc, char **argv) double ttrack= std::chrono::duration_cast >(t2 - t1).count(); ttrack_tot += ttrack; - // std::cout << "ttrack: " << ttrack << std::endl; vTimesTrack[ni]=ttrack; @@ -149,7 +148,7 @@ int main(int argc, char **argv) T = tframe-vTimestampsCam[seq][ni-1]; if(ttrack ### Related Publications: -[ORB-SLAM3] Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M. M. Montiel and Juan D. Tardós, **ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM**, Under review. **[PDF](https://arxiv.org/pdf/2007.11898.pdf)**. +[ORB-SLAM3] Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M. M. Montiel and Juan D. Tardós, **ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM**, *IEEE Transactions on Robotics, 2021* **[PDF](https://arxiv.org/abs/2007.11898)**. [IMU-Initialization] Carlos Campos, J. M. M. Montiel and Juan D. Tardós, **Inertial-Only Optimization for Visual-Inertial Initialization**, *ICRA 2020*. **[PDF](https://arxiv.org/pdf/2003.05766.pdf)** @@ -221,5 +221,6 @@ Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab. rosrun rosbag fastrebag.py dataset-room1_512_16.bag dataset-room1_512_16_small_chunks.bag ``` - +# 7. Time analysis +A flag in `include\Config.h` activates time measurements. It is necessary to uncomment the line `#define REGISTER_TIMES` to obtain the time stats of one execution which is shown at the terminal and stored in a text file(`ExecTimeMean.txt`). diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt index 0eb5126..c561724 100644 --- a/Thirdparty/DBoW2/CMakeLists.txt +++ b/Thirdparty/DBoW2/CMakeLists.txt @@ -24,11 +24,11 @@ set(SRCS_DUTILS DUtils/Random.cpp DUtils/Timestamp.cpp) -find_package(OpenCV 3.0 QUIET) +find_package(OpenCV 4.0 QUIET) if(NOT OpenCV_FOUND) - find_package(OpenCV 2.4.3 QUIET) + find_package(OpenCV 3.0 QUIET) if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 2.4.3 not found.") + message(FATAL_ERROR "OpenCV > 3.0 not found.") endif() endif() diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index 822c201..0195934 100644 --- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -1333,13 +1333,7 @@ int TemplatedVocabulary::stopWords(double minWeight) } // -------------------------------------------------------------------------- -/** - * @brief 读取TXT格式的预训练好的ORB字典 - * - * @param[in] filename 文件名 - * @return true 读取成功 - * @return false 读取失败 - */ + template bool TemplatedVocabulary::loadFromTextFile(const std::string &filename) { @@ -1347,7 +1341,7 @@ bool TemplatedVocabulary::loadFromTextFile(const std::string &fil f.open(filename.c_str()); if(f.eof()) - return false; + return false; m_words.clear(); m_nodes.clear(); @@ -1356,8 +1350,8 @@ bool TemplatedVocabulary::loadFromTextFile(const std::string &fil getline(f,s); stringstream ss; ss << s; - ss >> m_k; // 树的分支数目 - ss >> m_L; // 树的深度 + ss >> m_k; + ss >> m_L; int n1, n2; ss >> n1; ss >> n2; @@ -1365,22 +1359,20 @@ bool TemplatedVocabulary::loadFromTextFile(const std::string &fil 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; + return false; } - m_scoring = (ScoringType)n1; // 评分类型 - m_weighting = (WeightingType)n2; // 权重类型 + m_scoring = (ScoringType)n1; + m_weighting = (WeightingType)n2; createScoringObject(); - // 总共节点（nodes）数，是一个等比数列求和 - //! bug 没有包含最后叶子节点数，应该改为 ((pow((double)m_k, (double)m_L + 2) - 1)/(m_k - 1)) - //! 但是没有影响，因为这里只是reserve，实际存储是一步步resize实现 + // 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)); - // 第一个节点是根节点，id设为0 + m_nodes.resize(1); m_nodes[0].id = 0; while(!f.eof()) @@ -1389,53 +1381,40 @@ bool TemplatedVocabulary::loadFromTextFile(const std::string &fil getline(f,snode); stringstream ssnode; ssnode << snode; - // nid 表示当前节点id，实际是读取顺序，从0开始 - int nid = m_nodes.size(); - // 节点size 加1 - m_nodes.resize(m_nodes.size()+1); - m_nodes[nid].id = nid; - // 读每行的第1个数字，表示父节点id + int nid = m_nodes.size(); + m_nodes.resize(m_nodes.size()+1); + m_nodes[nid].id = nid; + int pid ; ssnode >> pid; - // 记录节点id的相互父子关系 m_nodes[nid].parent = pid; m_nodes[pid].children.push_back(nid); - // 读取第2个数字，表示是否是叶子（Word） int nIsLeaf; ssnode >> nIsLeaf; - // 每个特征点描述子是256 bit，一个字节对应8 bit，所以一个特征点需要32个字节存储。 - // 这里 F::L=32，也就是读取32个字节，最后以字符串形式存储在ssd stringstream ssd; for(int iD=0;iD> sElement; ssd << sElement << " "; - } - // 将ssd存储在该节点的描述子 + } F::fromString(m_nodes[nid].descriptor, ssd.str()); - // 读取最后一个数字：节点的权重（Word才有） ssnode >> m_nodes[nid].weight; if(nIsLeaf>0) { - // 如果是叶子（Word），存储到m_words int wid = m_words.size(); m_words.resize(wid+1); - //存储Word的id，具有唯一性 m_nodes[nid].word_id = wid; - - //构建 vector m_words，存储word所在node的指针 m_words[wid] = &m_nodes[nid]; } else { - //非叶子节点，直接分配 m_k个分支 m_nodes[nid].children.reserve(m_k); } } diff --git a/build.sh b/build.sh index a70a7f1..9aadf59 100644 --- a/build.sh +++ b/build.sh @@ -27,5 +27,5 @@ echo "Configuring and building ORB_SLAM3 ..." mkdir build cd build -cmake .. -DCMAKE_BUILD_TYPE=Debug -make +cmake .. -DCMAKE_BUILD_TYPE=Release +make -j diff --git a/include/Atlas.h b/include/Atlas.h index b8abb67..f89925e 100644 --- a/include/Atlas.h +++ b/include/Atlas.h @@ -43,33 +43,8 @@ class Frame; class KannalaBrandt8; class Pinhole; -//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") -//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") - class Atlas { - friend class boost::serialization::access; - - template - void serialize(Archive &ar, const unsigned int version) - { - //ar.template register_type(); - //ar.template register_type(); - - // Save/load the set of maps, the set is broken in libboost 1.58 for ubuntu 16.04 - //ar & mspMaps; - ar & mvpBackupMaps; - ar & mvpCameras; - //ar & mvpBackupCamPin; - //ar & mvpBackupCamKan; - // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map - ar & Map::nNextId; - ar & Frame::nNextId; - ar & KeyFrame::nNextId; - ar & MapPoint::nNextId; - ar & GeometricCamera::nNextId; - ar & mnLastInitKFidMap; - } public: Atlas(); @@ -86,8 +61,6 @@ public: // Method for change components in the current map void AddKeyFrame(KeyFrame* pKF); void AddMapPoint(MapPoint* pMP); - //void EraseMapPoint(MapPoint* pMP); - //void EraseKeyFrame(KeyFrame* pKF); void AddCamera(GeometricCamera* pCam); @@ -122,10 +95,6 @@ public: void SetImuInitialized(); bool isImuInitialized(); - // Function for garantee the correction of serialization of this object - void PreSave(); - void PostLoad(); - void SetKeyFrameDababase(KeyFrameDatabase* pKFDB); KeyFrameDatabase* GetKeyFrameDatabase(); @@ -140,15 +109,12 @@ protected: std::set mspMaps; std::set mspBadMaps; - // Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer - std::vector mvpBackupMaps; Map* mpCurrentMap; std::vector mvpCameras; std::vector mvpBackupCamKan; std::vector mvpBackupCamPin; - //Pinhole testCam; std::mutex mMutexAtlas; unsigned long int mnLastInitKFidMap; diff --git a/include/CameraModels/GeometricCamera.h b/include/CameraModels/GeometricCamera.h index 1149bed..49fff32 100644 --- a/include/CameraModels/GeometricCamera.h +++ b/include/CameraModels/GeometricCamera.h @@ -36,23 +36,13 @@ namespace ORB_SLAM3 { class GeometricCamera { - friend class boost::serialization::access; - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & mnId; - ar & mnType; - ar & mvParameters; - } - - public: GeometricCamera() {} GeometricCamera(const std::vector &_vParameters) : mvParameters(_vParameters) {} ~GeometricCamera() {} virtual cv::Point2f project(const cv::Point3f &p3D) = 0; + virtual cv::Point2f project(const cv::Matx31f &m3D) = 0; virtual cv::Point2f project(const cv::Mat& m3D) = 0; virtual Eigen::Vector2d project(const Eigen::Vector3d & v3D) = 0; virtual cv::Mat projectMat(const cv::Point3f& p3D) = 0; @@ -61,6 +51,7 @@ namespace ORB_SLAM3 { virtual cv::Point3f unproject(const cv::Point2f &p2D) = 0; virtual cv::Mat unprojectMat(const cv::Point2f &p2D) = 0; + virtual cv::Matx31f unprojectMat_(const cv::Point2f &p2D) = 0; virtual cv::Mat projectJac(const cv::Point3f &p3D) = 0; virtual Eigen::Matrix projectJac(const Eigen::Vector3d& v3D) = 0; @@ -71,8 +62,10 @@ namespace ORB_SLAM3 { cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated) = 0; virtual cv::Mat toK() = 0; + virtual cv::Matx33f toK_() = 0; virtual bool epipolarConstrain(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc) = 0; + virtual bool epipolarConstrain_(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc) = 0; float getParameter(const int i){return mvParameters[i];} void setParameter(const float p, const size_t i){mvParameters[i] = p;} diff --git a/include/CameraModels/KannalaBrandt8.h b/include/CameraModels/KannalaBrandt8.h index 9ac39ac..a5ef535 100644 --- a/include/CameraModels/KannalaBrandt8.h +++ b/include/CameraModels/KannalaBrandt8.h @@ -35,15 +35,6 @@ namespace ORB_SLAM3 { class KannalaBrandt8 final : public GeometricCamera { - friend class boost::serialization::access; - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & boost::serialization::base_object(*this); - ar & const_cast(precision); - } - public: KannalaBrandt8() : precision(1e-6) { mvParameters.resize(8); @@ -69,6 +60,7 @@ namespace ORB_SLAM3 { } cv::Point2f project(const cv::Point3f &p3D); + cv::Point2f project(const cv::Matx31f &m3D); cv::Point2f project(const cv::Mat& m3D); Eigen::Vector2d project(const Eigen::Vector3d & v3D); cv::Mat projectMat(const cv::Point3f& p3D); @@ -77,6 +69,7 @@ namespace ORB_SLAM3 { cv::Point3f unproject(const cv::Point2f &p2D); cv::Mat unprojectMat(const cv::Point2f &p2D); + cv::Matx31f unprojectMat_(const cv::Point2f &p2D); cv::Mat projectJac(const cv::Point3f &p3D); Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); @@ -87,10 +80,14 @@ namespace ORB_SLAM3 { cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); cv::Mat toK(); + cv::Matx33f toK_(); bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc); + bool epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc); + float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc, cv::Mat& p3D); + float TriangulateMatches_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc, cv::Matx31f& p3D); std::vector mvLappingArea; @@ -110,6 +107,7 @@ namespace ORB_SLAM3 { TwoViewReconstruction* tvr; void Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Mat &Tcw1, const cv::Mat &Tcw2,cv::Mat &x3D); + void Triangulate_(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Matx44f &Tcw1, const cv::Matx44f &Tcw2,cv::Matx31f &x3D); }; } diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h index 542ea66..01b703e 100644 --- a/include/CameraModels/Pinhole.h +++ b/include/CameraModels/Pinhole.h @@ -35,14 +35,6 @@ namespace ORB_SLAM3 { class Pinhole : public GeometricCamera { - friend class boost::serialization::access; - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & boost::serialization::base_object(*this); - } - public: Pinhole() { mvParameters.resize(4); @@ -67,6 +59,7 @@ namespace ORB_SLAM3 { } cv::Point2f project(const cv::Point3f &p3D); + cv::Point2f project(const cv::Matx31f &m3D); cv::Point2f project(const cv::Mat &m3D); Eigen::Vector2d project(const Eigen::Vector3d & v3D); cv::Mat projectMat(const cv::Point3f& p3D); @@ -75,6 +68,8 @@ namespace ORB_SLAM3 { cv::Point3f unproject(const cv::Point2f &p2D); cv::Mat unprojectMat(const cv::Point2f &p2D); + cv::Matx31f unprojectMat_(const cv::Point2f &p2D); + cv::Mat projectJac(const cv::Point3f &p3D); Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); @@ -85,8 +80,10 @@ namespace ORB_SLAM3 { cv::Mat &R21, cv::Mat &t21, std::vector &vP3D, std::vector &vbTriangulated); cv::Mat toK(); + cv::Matx33f toK_(); bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc); + bool epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc); bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, cv::Mat& Tcw1, cv::Mat& Tcw2, @@ -97,6 +94,7 @@ namespace ORB_SLAM3 { friend std::istream& operator>>(std::istream& os, Pinhole& ph); private: cv::Mat SkewSymmetricMatrix(const cv::Mat &v); + cv::Matx33f SkewSymmetricMatrix_(const cv::Matx31f &v); //Parameters vector corresponds to // [fx, fy, cx, cy] diff --git a/include/Frame.h b/include/Frame.h index c896bc2..07cfd60 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -20,8 +20,6 @@ #ifndef FRAME_H #define FRAME_H -//#define SAVE_TIMES - #include #include "Thirdparty/DBoW2/DBoW2/BowVector.h" @@ -29,6 +27,7 @@ #include "ImuTypes.h" #include "ORBVocabulary.h" +#include "Config.h" #include #include @@ -61,9 +60,6 @@ public: // Constructor for Monocular cameras. Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); - // Destructor - // ~Frame(); - // Extract ORB on the image. 0 for left image and 1 for right image. void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1); @@ -247,9 +243,10 @@ public: int mnDataset; - double mTimeStereoMatch; +#ifdef REGISTER_TIMES double mTimeORB_Ext; - + double mTimeStereoMatch; +#endif private: @@ -269,6 +266,10 @@ private: cv::Mat mtcw; //==mtwc + cv::Matx31f mOwx; + cv::Matx33f mRcwx; + cv::Matx31f mtcwx; + bool mbImuPreintegrated; std::mutex *mpMutexImu; @@ -295,6 +296,7 @@ public: std::vector mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS]; cv::Mat mTlr, mRlr, mtlr, mTrl; + cv::Matx34f mTrlx, mTlrx; Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, cv::Mat& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 578ba86..de9f00a 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -51,233 +51,6 @@ class GeometricCamera; class KeyFrame { - - template - void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version) - { - int cols, rows, type; - bool continuous; - - if (Archive::is_saving::value) { - cols = mat.cols; rows = mat.rows; type = mat.type(); - continuous = mat.isContinuous(); - } - - ar & cols & rows & type & continuous; - - if (Archive::is_loading::value) - mat.create(rows, cols, type); - - if (continuous) { - const unsigned int data_size = rows * cols * mat.elemSize(); - ar & boost::serialization::make_array(mat.ptr(), data_size); - } else { - const unsigned int row_size = cols*mat.elemSize(); - for (int i = 0; i < rows; i++) { - ar & boost::serialization::make_array(mat.ptr(i), row_size); - } - } - } - - - template - void serializeMatrix(Archive& ar, const cv::Mat& mat, const unsigned int version) - { - cv::Mat matAux = mat; - - serializeMatrix(ar, matAux,version); - - if (Archive::is_loading::value) - { - cv::Mat* ptr; - ptr = (cv::Mat*)( &mat ); - *ptr = matAux; - } - } - - friend class boost::serialization::access; - template - void serializeVectorKeyPoints(Archive& ar, const vector& vKP, const unsigned int version) - { - int NumEl; - - if (Archive::is_saving::value) { - NumEl = vKP.size(); - } - - ar & NumEl; - - vector vKPaux = vKP; - if (Archive::is_loading::value) - vKPaux.reserve(NumEl); - - for(int i=0; i < NumEl; ++i) - { - cv::KeyPoint KPi; - - if (Archive::is_loading::value) - KPi = cv::KeyPoint(); - - if (Archive::is_saving::value) - KPi = vKPaux[i]; - - ar & KPi.angle; - ar & KPi.response; - ar & KPi.size; - ar & KPi.pt.x; - ar & KPi.pt.y; - ar & KPi.class_id; - ar & KPi.octave; - - if (Archive::is_loading::value) - vKPaux.push_back(KPi); - } - - - if (Archive::is_loading::value) - { - vector *ptr; - ptr = (vector*)( &vKP ); - *ptr = vKPaux; - } - } - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & mnId; - ar & const_cast(mnFrameId); - ar & const_cast(mTimeStamp); - // Grid - ar & const_cast(mnGridCols); - ar & const_cast(mnGridRows); - ar & const_cast(mfGridElementWidthInv); - ar & const_cast(mfGridElementHeightInv); - // Variables of tracking - ar & mnTrackReferenceForFrame; - ar & mnFuseTargetForKF; - // Variables of local mapping - ar & mnBALocalForKF; - ar & mnBAFixedForKF; - ar & mnNumberOfOpt; - // Variables used by KeyFrameDatabase - ar & mnLoopQuery; - ar & mnLoopWords; - ar & mLoopScore; - ar & mnRelocQuery; - ar & mnRelocWords; - ar & mRelocScore; - ar & mnMergeQuery; - ar & mnMergeWords; - ar & mMergeScore; - ar & mnPlaceRecognitionQuery; - ar & mnPlaceRecognitionWords; - ar & mPlaceRecognitionScore; - ar & mbCurrentPlaceRecognition; - // Variables of loop closing - serializeMatrix(ar,mTcwGBA,version); - serializeMatrix(ar,mTcwBefGBA,version); - serializeMatrix(ar,mVwbGBA,version); - serializeMatrix(ar,mVwbBefGBA,version); - ar & mBiasGBA; - ar & mnBAGlobalForKF; - // Variables of Merging - serializeMatrix(ar,mTcwMerge,version); - serializeMatrix(ar,mTcwBefMerge,version); - serializeMatrix(ar,mTwcBefMerge,version); - serializeMatrix(ar,mVwbMerge,version); - serializeMatrix(ar,mVwbBefMerge,version); - ar & mBiasMerge; - ar & mnMergeCorrectedForKF; - ar & mnMergeForKF; - ar & mfScaleMerge; - ar & mnBALocalForMerge; - // Scale - ar & mfScale; - // Calibration parameters - ar & const_cast(fx); - ar & const_cast(fy); - ar & const_cast(invfx); - ar & const_cast(invfy); - ar & const_cast(cx); - ar & const_cast(cy); - ar & const_cast(mbf); - ar & const_cast(mb); - ar & const_cast(mThDepth); - serializeMatrix(ar,mDistCoef,version); - // Number of Keypoints - ar & const_cast(N); - // KeyPoints - serializeVectorKeyPoints(ar,mvKeys,version); - serializeVectorKeyPoints(ar,mvKeysUn,version); - ar & const_cast& >(mvuRight); - ar & const_cast& >(mvDepth); - serializeMatrix(ar,mDescriptors,version); - // BOW - ar & mBowVec; - ar & mFeatVec; - // Pose relative to parent - serializeMatrix(ar,mTcp,version); - // Scale - ar & const_cast(mnScaleLevels); - ar & const_cast(mfScaleFactor); - ar & const_cast(mfLogScaleFactor); - ar & const_cast& >(mvScaleFactors); - ar & const_cast& >(mvLevelSigma2); - ar & const_cast& >(mvInvLevelSigma2); - // Image bounds and calibration - ar & const_cast(mnMinX); - ar & const_cast(mnMinY); - ar & const_cast(mnMaxX); - ar & const_cast(mnMaxY); - serializeMatrix(ar,mK,version); - // Pose - serializeMatrix(ar,Tcw,version); - // MapPointsId associated to keypoints - ar & mvBackupMapPointsId; - // Grid - ar & mGrid; - // Connected KeyFrameWeight - ar & mBackupConnectedKeyFrameIdWeights; - // Spanning Tree and Loop Edges - ar & mbFirstConnection; - ar & mBackupParentId; - ar & mvBackupChildrensId; - ar & mvBackupLoopEdgesId; - ar & mvBackupMergeEdgesId; - // Bad flags - ar & mbNotErase; - ar & mbToBeErased; - ar & mbBad; - - ar & mHalfBaseline; - - // Camera variables - ar & mnBackupIdCamera; - ar & mnBackupIdCamera2; - - // Fisheye variables - /*ar & mvLeftToRightMatch; - ar & mvRightToLeftMatch; - ar & NLeft; - ar & NRight; - serializeMatrix(ar, mTlr, version); - //serializeMatrix(ar, mTrl, version); - serializeVectorKeyPoints(ar, mvKeysRight, version); - ar & mGridRight; - - // Inertial variables - ar & mImuBias; - ar & mBackupImuPreintegrated; - ar & mImuCalib; - ar & mBackupPrevKFId; - ar & mBackupNextKFId; - ar & bImu; - serializeMatrix(ar, Vw, version); - serializeMatrix(ar, Owb, version);*/ - - } - public: KeyFrame(); KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); @@ -297,6 +70,16 @@ public: cv::Mat GetTranslation(); cv::Mat GetVelocity(); + cv::Matx33f GetRotation_(); + cv::Matx31f GetTranslation_(); + cv::Matx31f GetCameraCenter_(); + cv::Matx33f GetRightRotation_(); + cv::Matx31f GetRightTranslation_(); + cv::Matx44f GetRightPose_(); + cv::Matx31f GetRightCameraCenter_(); + cv::Matx44f GetPose_(); + + // Bag of Words Representation void ComputeBoW(); @@ -343,6 +126,7 @@ public: // KeyPoint functions std::vector GetFeaturesInArea(const float &x, const float &y, const float &r, const bool bRight = false) const; cv::Mat UnprojectStereo(int i); + cv::Matx31f UnprojectStereo_(int i); // Image bool IsInImage(const float &x, const float &y) const; @@ -377,10 +161,6 @@ public: bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v); - void PreSave(set& spKF,set& spMP, set& spCam); - void PostLoad(map& mpKFid, map& mpMPid, map& mpCamId); - - void SetORBVocabulary(ORBVocabulary* pORBVoc); void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB); @@ -516,6 +296,9 @@ protected: cv::Mat Ow; cv::Mat Cw; // Stereo middel point. Only for visualization + cv::Matx44f Tcw_, Twc_, Tlr_; + cv::Matx31f Ow_; + // IMU position cv::Mat Owb; @@ -527,8 +310,6 @@ protected: // MapPoints associated to keypoints std::vector mvpMapPoints; - // For save relation without pointer, this is necessary for save/load function - std::vector mvBackupMapPointsId; // BoW KeyFrameDatabase* mpKeyFrameDB; @@ -540,8 +321,6 @@ protected: std::map mConnectedKeyFrameWeights; std::vector mvpOrderedConnectedKeyFrames; std::vector mvOrderedWeights; - // For save relation without pointer, this is necessary for save/load function - std::map mBackupConnectedKeyFrameIdWeights; // Spanning Tree and Loop Edges bool mbFirstConnection; @@ -549,11 +328,6 @@ protected: std::set mspChildrens; std::set mspLoopEdges; std::set mspMergeEdges; - // For save relation without pointer, this is necessary for save/load function - long long int mBackupParentId; - std::vector mvBackupChildrensId; - std::vector mvBackupLoopEdgesId; - std::vector mvBackupMergeEdgesId; // Bad flags bool mbNotErase; @@ -569,14 +343,6 @@ protected: std::mutex mMutexFeatures; std::mutex mMutexMap; - // Backup variables for inertial - long long int mBackupPrevKFId; - long long int mBackupNextKFId; - IMU::Preintegrated mBackupImuPreintegrated; - - // Backup for Cameras - unsigned int mnBackupIdCamera, mnBackupIdCamera2; - public: GeometricCamera* mpCamera, *mpCamera2; @@ -601,7 +367,7 @@ public: cv::Mat GetRightRotation(); cv::Mat GetRightTranslation(); - cv::Mat imgLeft, imgRight; //TODO Backup?? + cv::Mat imgLeft, imgRight; void PrintPointDistribution(){ int left = 0, right = 0; diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h index f991020..0df7902 100644 --- a/include/KeyFrameDatabase.h +++ b/include/KeyFrameDatabase.h @@ -46,13 +46,6 @@ class Map; class KeyFrameDatabase { - friend class boost::serialization::access; - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & mvBackupInvertedFileId; - } public: @@ -76,8 +69,6 @@ public: // Relocalization std::vector DetectRelocalizationCandidates(Frame* F, Map* pMap); - void PreSave(); - void PostLoad(map mpKFid); void SetORBVocabulary(ORBVocabulary* pORBVoc); protected: @@ -88,9 +79,6 @@ protected: // Inverted file std::vector > mvInvertedFile; - // For save relation without pointer, this is necessary for save/load function - std::vector > mvBackupInvertedFileId; - // Mutex std::mutex mMutex; }; diff --git a/include/LocalMapping.h b/include/LocalMapping.h index 11330e8..e5a21ad 100644 --- a/include/LocalMapping.h +++ b/include/LocalMapping.h @@ -95,11 +95,6 @@ public: double mFirstTs; int mnMatchesInliers; - // For debugging (erase in normal mode) - int mInitFr; - int mIdxIteration; - string strSequence; - bool mbNotBA1; bool mbNotBA2; bool mbBadImu; @@ -109,6 +104,25 @@ public: // not consider far points (clouds) bool mbFarPoints; float mThFarPoints; + +#ifdef REGISTER_TIMES + vector vdKFInsert_ms; + vector vdMPCulling_ms; + vector vdMPCreation_ms; + vector vdLBA_ms; + vector vdKFCulling_ms; + vector vdLMTotal_ms; + + + vector vdLBASync_ms; + vector vdKFCullingSync_ms; + vector vnLBA_edges; + vector vnLBA_KFopt; + vector vnLBA_KFfixed; + vector vnLBA_MPs; + int nLBA_exec; + int nLBA_abort; +#endif protected: bool CheckNewKeyFrames(); @@ -120,8 +134,10 @@ protected: void KeyFrameCulling(); cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); + cv::Matx33f ComputeF12_(KeyFrame* &pKF1, KeyFrame* &pKF2); cv::Mat SkewSymmetricMatrix(const cv::Mat &v); + cv::Matx33f SkewSymmetricMatrix_(const cv::Matx31f &v); System *mpSystem; diff --git a/include/LoopClosing.h b/include/LoopClosing.h index 4be8b77..7ce6392 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -25,6 +25,7 @@ #include "Atlas.h" #include "ORBVocabulary.h" #include "Tracking.h" +#include "Config.h" #include "KeyFrameDatabase.h" @@ -48,7 +49,7 @@ public: typedef pair,int> ConsistentGroup; typedef map, - Eigen::aligned_allocator > > KeyFrameAndPose; + Eigen::aligned_allocator > > KeyFrameAndPose; public: @@ -86,6 +87,26 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#ifdef REGISTER_TIMES + double timeDetectBoW; + + std::vector vTimeBoW_ms; + std::vector vTimeSE3_ms; + std::vector vTimePRTotal_ms; + + std::vector vTimeLoopFusion_ms; + std::vector vTimeLoopEssent_ms; + std::vector vTimeLoopTotal_ms; + + std::vector vTimeMergeFusion_ms; + std::vector vTimeMergeBA_ms; + std::vector vTimeMergeTotal_ms; + + std::vector vTimeFullGBA_ms; + std::vector vTimeMapUpdate_ms; + std::vector vTimeGBATotal_ms; +#endif + protected: bool CheckNewKeyFrames(); @@ -112,9 +133,6 @@ protected: void MergeLocal(); void MergeLocal2(); - void CheckObservations(set &spKFsMap1, set &spKFsMap2); - void printReprojectionError(set &spLocalWindowKFs, KeyFrame* mpCurrentKF, string &name); - void ResetIfRequested(); bool mbResetRequested; bool mbResetActiveMapRequested; diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h index cede745..37c3811 100644 --- a/include/MLPnPsolver.h +++ b/include/MLPnPsolver.h @@ -65,8 +65,6 @@ namespace ORB_SLAM3{ void SetRansacParameters(double probability = 0.99, int minInliers = 8, int maxIterations = 300, int minSet = 6, float epsilon = 0.4, float th2 = 5.991); - //Find metod is necessary? - cv::Mat iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers); //Type definitions needed by the original code diff --git a/include/Map.h b/include/Map.h index 4c4314d..799d49d 100644 --- a/include/Map.h +++ b/include/Map.h @@ -37,39 +37,10 @@ class MapPoint; class KeyFrame; class Atlas; class KeyFrameDatabase; +class GeometricCamera; class Map { - friend class boost::serialization::access; - - template - void serialize(Archive &ar, const unsigned int version) - { - ar & mnId; - ar & mnInitKFid; - ar & mnMaxKFid; - ar & mnBigChangeIdx; - // Set of KeyFrames and MapPoints, in this version the set serializator is not working - //ar & mspKeyFrames; - //ar & mspMapPoints; - - ar & mvpBackupKeyFrames; - ar & mvpBackupMapPoints; - - ar & mvBackupKeyFrameOriginsId; - - ar & mnBackupKFinitialID; - ar & mnBackupKFlowerID; - - ar & mbImuInitialized; - ar & mbIsInertial; - ar & mbIMU_BA1; - ar & mbIMU_BA2; - - ar & mnInitKFid; - ar & mnMaxKFid; - ar & mnLastLoopKFid; - } public: Map(); @@ -134,11 +105,6 @@ public: unsigned int GetLowerKFID(); - void PreSave(std::set &spCams); - void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map& mpKeyFrameId, map &mpCams); - - void printReprojectionError(list &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder); - vector mvpKeyFrameOrigins; vector mvBackupKeyFrameOriginsId; KeyFrame* mpFirstRegionKF; @@ -162,15 +128,9 @@ protected: std::set mspMapPoints; std::set mspKeyFrames; - std::vector mvpBackupMapPoints; - std::vector mvpBackupKeyFrames; - KeyFrame* mpKFinitial; KeyFrame* mpKFlowerID; - unsigned long int mnBackupKFinitialID; - unsigned long int mnBackupKFlowerID; - std::vector mvpReferenceMapPoints; bool mbImuInitialized; @@ -193,7 +153,7 @@ protected: bool mHasTumbnail; bool mbBad = false; - bool mbIsInertial; //标记是否有imu + bool mbIsInertial; bool mbIMU_BA1; bool mbIMU_BA2; diff --git a/include/MapPoint.h b/include/MapPoint.h index 4ab4180..b7b1f57 100644 --- a/include/MapPoint.h +++ b/include/MapPoint.h @@ -40,89 +40,6 @@ class Frame; class MapPoint { - template - void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version) - { - int cols, rows, type; - bool continuous; - - if (Archive::is_saving::value) { - cols = mat.cols; rows = mat.rows; type = mat.type(); - continuous = mat.isContinuous(); - } - - ar & cols & rows & type & continuous; - if (Archive::is_loading::value) - mat.create(rows, cols, type); - - if (continuous) { - const unsigned int data_size = rows * cols * mat.elemSize(); - ar & boost::serialization::make_array(mat.ptr(), data_size); - } else { - const unsigned int row_size = cols*mat.elemSize(); - for (int i = 0; i < rows; i++) { - ar & boost::serialization::make_array(mat.ptr(i), row_size); - } - } - } - - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & mnId; - ar & mnFirstKFid; - ar & mnFirstFrame; - ar & nObs; - // Variables used by the tracking - ar & mTrackProjX; - ar & mTrackProjY; - ar & mTrackDepth; - ar & mTrackDepthR; - ar & mTrackProjXR; - ar & mTrackProjYR; - ar & mbTrackInView; - ar & mbTrackInViewR; - ar & mnTrackScaleLevel; - ar & mnTrackScaleLevelR; - ar & mTrackViewCos; - ar & mTrackViewCosR; - ar & mnTrackReferenceForFrame; - ar & mnLastFrameSeen; - - // Variables used by local mapping - ar & mnBALocalForKF; - ar & mnFuseCandidateForKF; - - // Variables used by loop closing and merging - ar & mnLoopPointForKF; - ar & mnCorrectedByKF; - ar & mnCorrectedReference; - serializeMatrix(ar,mPosGBA,version); - ar & mnBAGlobalForKF; - ar & mnBALocalForMerge; - serializeMatrix(ar,mPosMerge,version); - serializeMatrix(ar,mNormalVectorMerge,version); - - // Protected variables - serializeMatrix(ar,mWorldPos,version); - //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId); - ar & mBackupObservationsId1; - ar & mBackupObservationsId2; - serializeMatrix(ar,mNormalVector,version); - serializeMatrix(ar,mDescriptor,version); - ar & mBackupRefKFId; - ar & mnVisible; - ar & mnFound; - - ar & mbBad; - ar & mBackupReplacedId; - - ar & mfMinDistance; - ar & mfMaxDistance; - - } - public: MapPoint(); @@ -136,6 +53,11 @@ public: cv::Mat GetWorldPos(); cv::Mat GetNormal(); + + cv::Matx31f GetWorldPos2(); + + cv::Matx31f GetNormal2(); + KeyFrame* GetReferenceKeyFrame(); std::map> GetObservations(); @@ -175,11 +97,6 @@ public: Map* GetMap(); void UpdateMap(Map* pMap); - void PrintObservations(); - - void PreSave(set& spKF,set& spMP); - void PostLoad(map& mpKFid, map& mpMPid); - public: long unsigned int mnId; static long unsigned int nNextId; @@ -231,22 +148,20 @@ protected: // Position in absolute coordinates cv::Mat mWorldPos; + cv::Matx31f mWorldPosx; // Keyframes observing the point and associated index in keyframe std::map > mObservations; - // For save relation without pointer, this is necessary for save/load function - std::map mBackupObservationsId1; - std::map mBackupObservationsId2; // Mean viewing direction cv::Mat mNormalVector; + cv::Matx31f mNormalVectorx; // Best descriptor to fast matching cv::Mat mDescriptor; // Reference KeyFrame KeyFrame* mpRefKF; - long unsigned int mBackupRefKFId; // Tracking counters int mnVisible; @@ -255,8 +170,6 @@ protected: // Bad flag (we do not currently erase MapPoint from memory) bool mbBad; MapPoint* mpReplaced; - // For save relation without pointer, this is necessary for save/load function - long long int mBackupReplacedId; // Scale invariance distances float mfMinDistance; diff --git a/include/ORBextractor.h b/include/ORBextractor.h index 8c2a15a..e687ed9 100644 --- a/include/ORBextractor.h +++ b/include/ORBextractor.h @@ -21,7 +21,7 @@ #include #include -#include +#include namespace ORB_SLAM3 diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h index 44241b1..b189838 100644 --- a/include/ORBmatcher.h +++ b/include/ORBmatcher.h @@ -74,6 +74,9 @@ public: int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, std::vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false); + int SearchForTriangulation_(KeyFrame *pKF1, KeyFrame* pKF2, cv::Matx33f F12, + std::vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false); + int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12, vector > &vMatchedPairs, const bool bOnlyStereo, vector &vMatchedPoints); diff --git a/include/Optimizer.h b/include/Optimizer.h index f56c1a0..87565fa 100644 --- a/include/Optimizer.h +++ b/include/Optimizer.h @@ -55,7 +55,7 @@ public: void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKF=0, bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL); void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, vector &vpNonEnoughOptKFs); - void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF); + void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges); void static MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vector vpWeldingKFs, vector vpFixedKFs, bool *pbStopFlag); @@ -96,7 +96,7 @@ public: // For inertial systems - void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, bool bLarge = false, bool bRecInit = false); + void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge = false, bool bRecInit = false); void static MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses); diff --git a/include/System.h b/include/System.h index 41a150b..4395710 100644 --- a/include/System.h +++ b/include/System.h @@ -20,8 +20,6 @@ #ifndef SYSTEM_H #define SYSTEM_H -//#define SAVE_TIMES - #include #include #include @@ -39,6 +37,7 @@ #include "ORBVocabulary.h" #include "Viewer.h" #include "ImuTypes.h" +#include "Config.h" namespace ORB_SLAM3 @@ -162,9 +161,6 @@ public: void SaveTrajectoryEuRoC(const string &filename); void SaveKeyFrameTrajectoryEuRoC(const string &filename); - // Save data used for initialization debug - void SaveDebugData(const int &iniIdx); - // Save camera trajectory in the KITTI dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() @@ -188,14 +184,14 @@ public: void ChangeDataset(); - //void SaveAtlas(int type); +#ifdef REGISTER_TIMES + void InsertRectTime(double& time); + + void InsertTrackTime(double& time); +#endif private: - //bool LoadAtlas(string filename, int type); - - //string CalculateCheckSum(string filename, int type); - // Input sensor eSensor mSensor; @@ -205,8 +201,7 @@ private: // KeyFrame database for place recognition (relocalization and loop detection). KeyFrameDatabase* mpKeyFrameDatabase; - // Map structure that stores the pointers to all KeyFrames and MapPoints. - //Map* mpMap; + // Atlas structure that stores the pointers to all KeyFrames and MapPoints. Atlas* mpAtlas; // Tracker. It receives a frame and computes the associated camera pose. diff --git a/include/Tracking.h b/include/Tracking.h index 8b08e6a..f88704e 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -159,14 +159,30 @@ public: vector GetLocalMapMPS(); - - //TEST-- - bool mbNeedRectify; - //cv::Mat M1l, M2l; - //cv::Mat M1r, M2r; - bool mbWriteStats; +#ifdef REGISTER_TIMES + void LocalMapStats2File(); + void TrackStats2File(); + void PrintTimeStats(); + + vector vdRectStereo_ms; + vector vdORBExtract_ms; + vector vdStereoMatch_ms; + vector vdIMUInteg_ms; + vector vdPosePred_ms; + vector vdLMTrack_ms; + vector vdNewKF_ms; + vector vdTrackTotal_ms; + + vector vdUpdatedLM_ms; + vector vdSearchLP_ms; + vector vdPoseOpt_ms; +#endif + + vector vnKeyFramesLM; + vector vnMapPointsLM; + protected: // Main tracking function. It is independent of the input sensor. @@ -204,7 +220,6 @@ protected: void PreintegrateIMU(); // Reset IMU biases and compute frame velocity - void ResetFrameIMU(); void ComputeGyroBias(const vector &vpFs, float &bwx, float &bwy, float &bwz); void ComputeVelocitiesAccBias(const vector &vpFs, float &bax, float &bay, float &baz); @@ -296,6 +311,7 @@ protected: unsigned int mnLastRelocFrameId; double mTimeStampLost; double time_recently_lost; + double time_recently_lost_visual; unsigned int mnFirstFrameId; unsigned int mnInitialFrameId; diff --git a/src/Atlas.cc b/src/Atlas.cc index 6b131af..a003ae0 100644 --- a/src/Atlas.cc +++ b/src/Atlas.cc @@ -260,81 +260,6 @@ bool Atlas::isImuInitialized() return mpCurrentMap->isImuInitialized(); } -void Atlas::PreSave() -{ - if(mpCurrentMap){ - if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid()) - mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum - } - - struct compFunctor - { - inline bool operator()(Map* elem1 ,Map* elem2) - { - return elem1->GetId() < elem2->GetId(); - } - }; - std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps)); - sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor()); - - std::set spCams(mvpCameras.begin(), mvpCameras.end()); - cout << "There are " << spCams.size() << " cameras in the atlas" << endl; - for(Map* pMi : mvpBackupMaps) - { - cout << "Pre-save of map " << pMi->GetId() << endl; - pMi->PreSave(spCams); - } - cout << "Maps stored" << endl; - for(GeometricCamera* pCam : mvpCameras) - { - cout << "Pre-save of camera " << pCam->GetId() << endl; - if(pCam->GetType() == pCam->CAM_PINHOLE) - { - mvpBackupCamPin.push_back((Pinhole*) pCam); - } - else if(pCam->GetType() == pCam->CAM_FISHEYE) - { - mvpBackupCamKan.push_back((KannalaBrandt8*) pCam); - } - } - -} - -void Atlas::PostLoad() -{ - mvpCameras.clear(); - map mpCams; - for(Pinhole* pCam : mvpBackupCamPin) - { - //mvpCameras.push_back((GeometricCamera*)pCam); - mvpCameras.push_back(pCam); - mpCams[pCam->GetId()] = pCam; - } - for(KannalaBrandt8* pCam : mvpBackupCamKan) - { - //mvpCameras.push_back((GeometricCamera*)pCam); - mvpCameras.push_back(pCam); - mpCams[pCam->GetId()] = pCam; - } - - mspMaps.clear(); - unsigned long int numKF = 0, numMP = 0; - map mpAllKeyFrameId; - for(Map* pMi : mvpBackupMaps) - { - cout << "Map id:" << pMi->GetId() << endl; - mspMaps.insert(pMi); - map mpKeyFrameId; - pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpKeyFrameId, mpCams); - mpAllKeyFrameId.insert(mpKeyFrameId.begin(), mpKeyFrameId.end()); - numKF += pMi->GetAllKeyFrames().size(); - numMP += pMi->GetAllMapPoints().size(); - } - - cout << "Number KF:" << numKF << "; number MP:" << numMP << endl; - mvpBackupMaps.clear(); -} - void Atlas::SetKeyFrameDababase(KeyFrameDatabase* pKFDB) { mpKeyFrameDB = pKFDB; diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp index 6651fdc..6876ad5 100644 --- a/src/CameraModels/KannalaBrandt8.cpp +++ b/src/CameraModels/KannalaBrandt8.cpp @@ -20,10 +20,7 @@ #include -//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8) - namespace ORB_SLAM3 { -//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) { const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y; @@ -43,6 +40,10 @@ namespace ORB_SLAM3 { } + cv::Point2f KannalaBrandt8::project(const cv::Matx31f &m3D) { + return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2))); + } + cv::Point2f KannalaBrandt8::project(const cv::Mat &m3D) { const float* p3D = m3D.ptr(); @@ -67,14 +68,6 @@ namespace ORB_SLAM3 { res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3]; return res; - - /*cv::Point2f cvres = this->project(cv::Point3f(v3D[0],v3D[1],v3D[2])); - - Eigen::Vector2d res; - res[0] = cvres.x; - res[1] = cvres.y; - - return res;*/ } cv::Mat KannalaBrandt8::projectMat(const cv::Point3f &p3D) { @@ -85,12 +78,6 @@ namespace ORB_SLAM3 { float KannalaBrandt8::uncertainty2(const Eigen::Matrix &p2D) { - /*Eigen::Matrix c; - c << mvParameters[2], mvParameters[3]; - if ((p2D-c).squaredNorm()>57600) // 240*240 (256) - return 100.f; - else - return 1.0f;*/ return 1.f; } @@ -100,6 +87,12 @@ namespace ORB_SLAM3 { return ret.clone(); } + cv::Matx31f KannalaBrandt8::unprojectMat_(const cv::Point2f &p2D) { + cv::Point3f ray = this->unproject(p2D); + cv::Matx31f r{ray.x, ray.y, ray.z}; + return r; + } + cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) { //Use Newthon method to solve for theta with good precision (err ~ e-6) cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]); @@ -232,11 +225,22 @@ namespace ORB_SLAM3 { return K; } + cv::Matx33f KannalaBrandt8::toK_() { + cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f}; + + return K; + } + bool KannalaBrandt8::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) { cv::Mat p3D; return this->TriangulateMatches(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f; } + bool KannalaBrandt8::epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) { + cv::Matx31f p3D; + return this->TriangulateMatches_(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f; + } + bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther, cv::Mat& Tcw1, cv::Mat& Tcw2, const float sigmaLevel1, const float sigmaLevel2, @@ -357,8 +361,8 @@ namespace ORB_SLAM3 { cv::Mat x3D; cv::Mat Tcw1 = (cv::Mat_(3,4) << 1.f,0.f,0.f,0.f, - 0.f,1.f,0.f,0.f, - 0.f,0.f,1.f,0.f); + 0.f,1.f,0.f,0.f, + 0.f,0.f,1.f,0.f); cv::Mat Tcw2; cv::Mat R21 = R12.t(); cv::Mat t21 = -R21*t12; @@ -402,6 +406,79 @@ namespace ORB_SLAM3 { return z1; } + float KannalaBrandt8::TriangulateMatches_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc, cv::Matx31f& p3D) { + cv::Matx31f r1 = this->unprojectMat_(kp1.pt); + cv::Matx31f r2 = pCamera2->unprojectMat_(kp2.pt); + + //Check parallax + cv::Matx31f r21 = R12*r2; + + const float cosParallaxRays = r1.dot(r21)/(cv::norm(r1)*cv::norm(r21)); + + if(cosParallaxRays > 0.9998){ + return -1; + } + + //Parallax is good, so we try to triangulate + cv::Point2f p11,p22; + + p11.x = r1(0); + p11.y = r1(1); + + p22.x = r2(0); + p22.y = r2(1); + + cv::Matx31f x3D; + cv::Matx44f Tcw1{1.f,0.f,0.f,0.f, + 0.f,1.f,0.f,0.f, + 0.f,0.f,1.f,0.f}; + + cv::Matx33f R21 = R12.t(); + cv::Matx31f t21 = -R21*t12; + + cv::Matx44f Tcw2{R21(0,0),R21(0,1),R21(0,2),t21(0), + R21(1,0),R21(1,1),R21(1,2),t21(1), + R21(2,0),R21(2,1),R21(2,2),t21(2), + 0.f,0.f,0.f,1.f}; + + Triangulate_(p11,p22,Tcw1,Tcw2,x3D); + cv::Matx13f x3Dt = x3D.t(); + + float z1 = x3D(2); + if(z1 <= 0){ + return -1; + } + + float z2 = R21.row(2).dot(x3Dt)+t21(2); + if(z2<=0){ + return -1; + } + + //Check reprojection error + cv::Point2f uv1 = this->project(x3D); + + float errX1 = uv1.x - kp1.pt.x; + float errY1 = uv1.y - kp1.pt.y; + + if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){ //Reprojection error is high + return -1; + } + + cv::Matx31f x3D2 = R21 * x3D + t21; + cv::Point2f uv2 = pCamera2->project(x3D2); + + float errX2 = uv2.x - kp2.pt.x; + float errY2 = uv2.y - kp2.pt.y; + + if((errX2*errX2+errY2*errY2)>5.991 * unc){ //Reprojection error is high + return -1; + } + + p3D = x3D; + + return z1; + } + std::ostream & operator<<(std::ostream &os, const KannalaBrandt8 &kb) { os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " " << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7]; @@ -433,4 +510,27 @@ namespace ORB_SLAM3 { x3D = vt.row(3).t(); x3D = x3D.rowRange(0,3)/x3D.at(3); } + + void KannalaBrandt8::Triangulate_(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Matx44f &Tcw1, const cv::Matx44f &Tcw2, cv::Matx31f &x3D) + { + cv::Matx14f A0, A1, A2, A3; + + + A0 = p1.x * Tcw1.row(2) - Tcw1.row(0); + A1 = p1.y * Tcw1.row(2) - Tcw1.row(1); + A2 = p2.x * Tcw2.row(2) - Tcw2.row(0); + A3 = p2.y * Tcw2.row(2) - Tcw2.row(1); + cv::Matx44f A{A0(0), A0(1), A0(2), A0(3), + A1(0), A1(1), A1(2), A1(3), + A2(0), A2(1), A2(2), A2(3), + A3(0), A3(1), A3(2), A3(3)}; + + // cv::Mat u,w,vt; + cv::Matx44f u, vt; + cv::Matx41f w; + + cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV); + cv::Matx41f x3D_h = vt.row(3).t(); + x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3); + } } diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp index ab9a8b4..d94ebb7 100644 --- a/src/CameraModels/Pinhole.cpp +++ b/src/CameraModels/Pinhole.cpp @@ -20,10 +20,7 @@ #include -//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole) - namespace ORB_SLAM3 { -//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") long unsigned int GeometricCamera::nNextId=0; @@ -32,6 +29,10 @@ namespace ORB_SLAM3 { mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); } + cv::Point2f Pinhole::project(const cv::Matx31f &m3D) { + return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2))); + } + cv::Point2f Pinhole::project(const cv::Mat &m3D) { const float* p3D = m3D.ptr(); @@ -66,6 +67,12 @@ namespace ORB_SLAM3 { return (cv::Mat_(3,1) << ray.x, ray.y, ray.z); } + cv::Matx31f Pinhole::unprojectMat_(const cv::Point2f &p2D) { + cv::Point3f ray = this->unproject(p2D); + cv::Matx31f r{ray.x, ray.y, ray.z}; + return r; + } + cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) { cv::Mat Jac(2, 3, CV_32F); Jac.at(0, 0) = mvParameters[0] / p3D.z; @@ -119,6 +126,12 @@ namespace ORB_SLAM3 { return K; } + cv::Matx33f Pinhole::toK_() { + cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f}; + + return K; + } + bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) { //Compute Fundamental Matrix cv::Mat t12x = SkewSymmetricMatrix(t12); @@ -143,6 +156,30 @@ namespace ORB_SLAM3 { return dsqr<3.84*unc; } + bool Pinhole::epipolarConstrain_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) { + //Compute Fundamental Matrix + auto t12x = SkewSymmetricMatrix_(t12); + auto K1 = this->toK_(); + auto K2 = pCamera2->toK_(); + cv::Matx33f F12 = K1.t().inv()*t12x*R12*K2.inv(); + + // Epipolar line in second image l = x1'F12 = [a b c] + const float a = kp1.pt.x*F12(0,0)+kp1.pt.y*F12(1,0)+F12(2,0); + const float b = kp1.pt.x*F12(0,1)+kp1.pt.y*F12(1,1)+F12(2,1); + const float c = kp1.pt.x*F12(0,2)+kp1.pt.y*F12(1,2)+F12(2,2); + + const float num = a*kp2.pt.x+b*kp2.pt.y+c; + + const float den = a*a+b*b; + + if(den==0) + return false; + + const float dsqr = num*num/den; + + return dsqr<3.84*unc; + } + std::ostream & operator<<(std::ostream &os, const Pinhole &ph) { os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3]; return os; @@ -165,4 +202,13 @@ namespace ORB_SLAM3 { v.at(2), 0,-v.at(0), -v.at(1), v.at(0), 0); } + + cv::Matx33f Pinhole::SkewSymmetricMatrix_(const cv::Matx31f &v) + { + cv::Matx33f skew{0.f, -v(2), v(1), + v(2), 0.f, -v(0), + -v(1), v(0), 0.f}; + + return skew; + } } diff --git a/src/Frame.cc b/src/Frame.cc index 562453e..6a462b0 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -49,6 +49,10 @@ cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING); Frame::Frame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false) { +#ifdef REGISTER_TIMES + mTimeStereoMatch = 0; + mTimeORB_Ext = 0; +#endif } @@ -69,7 +73,8 @@ Frame::Frame(const Frame &frame) mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), Nright(frame.Nright), monoLeft(frame.monoLeft), monoRight(frame.monoRight), mvLeftToRightMatch(frame.mvLeftToRightMatch), mvRightToLeftMatch(frame.mvRightToLeftMatch), mvStereo3Dpoints(frame.mvStereo3Dpoints), - mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()), mTimeStereoMatch(frame.mTimeStereoMatch), mTimeORB_Ext(frame.mTimeORB_Ext) + mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()), + mTrlx(frame.mTrlx), mTlrx(frame.mTlrx), mOwx(frame.mOwx), mRcwx(frame.mRcwx), mtcwx(frame.mtcwx) { for(int i=0;i(NULL)), mbImuPreintegrated(false), - mpCamera(pCamera) ,mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0) + mpCamera(pCamera) ,mpCamera2(nullptr) { // Step 1 帧的ID 自增 mnId=nNextId++; @@ -115,13 +125,21 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction - // Step 3 对左目右目图像提取ORB特征点, 第一个参数0-左图， 1-右图。为加速计算，同时开了两个线程计算 + // Step 3 对左目右目图像提取ORB特征点, 第一个参数0-左图， 1-右图。为加速计算，同时开了两个线程计算 +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif thread threadLeft(&Frame::ExtractORB,this,0,imLeft,0,0); // 对右目图像提取orb特征 thread threadRight(&Frame::ExtractORB,this,1,imRight,0,0); //等待两张图像特征点提取过程完成 threadLeft.join(); threadRight.join(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif //mvKeys中保存的是左图像中的特征点，这里是获取左侧图像中特征点的个数 N = mvKeys.size(); @@ -133,13 +151,20 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt // Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 UndistortKeyPoints(); - // Step 5 计算双目间特征点的匹配，只有匹配成功的特征点会计算其深度,深度存放在 mvDepth +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now(); +#endif + // Step 5 计算双目间特征点的匹配，只有匹配成功的特征点会计算其深度,深度存放在 mvDepth // mvuRight中存储的应该是左图像中的点所匹配的在右图像中的点的横坐标（纵坐标相同） ComputeStereoMatches(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now(); - // 初始化本帧的地图点 - mvpMapPoints = vector(N,static_cast(NULL)); - // 记录地图点是否为外点，初始化均为外点false + mTimeStereoMatch = std::chrono::duration_cast >(time_EndStereoMatches - time_StartStereoMatches).count(); +#endif + + // 初始化本帧的地图点 + mvpMapPoints = vector(N,static_cast(NULL)); mvbOutlier = vector(N,false); mmProjectPoints.clear();// = map(N, static_cast(NULL)); mmMatchedInImage.clear(); @@ -204,7 +229,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), - mpCamera(pCamera),mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0) + mpCamera(pCamera),mpCamera2(nullptr) { // Step 1 帧的ID 自增 mnId=nNextId++; @@ -225,11 +250,11 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction -#ifdef SAVE_TIMES +#ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif ExtractORB(0,imGray,0,0); -#ifdef SAVE_TIMES +#ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); @@ -305,7 +330,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(static_cast(pCamera)->toK()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), - mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0) + mpCamera2(nullptr) { // Frame ID // Step 1 帧的ID 自增 @@ -329,12 +354,12 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction -#ifdef SAVE_TIMES +#ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif // Step 3 对这个单目图像进行提取特征点, 第一个参数0-左图， 1-右图 ExtractORB(0,imGray,0,1000); -#ifdef SAVE_TIMES +#ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); @@ -532,6 +557,14 @@ void Frame::UpdatePoseMatrices() // mTcw 求逆后是当前相机坐标系变换到世界坐标系下，对应的光心变换到世界坐标系下就是 mTcw的逆 中对应的平移向量 mOw = -mRcw.t()*mtcw; + + // Static matrix + mOwx = cv::Matx31f(mOw.at(0), mOw.at(1), mOw.at(2)); + mRcwx = cv::Matx33f(mRcw.at(0,0), mRcw.at(0,1), mRcw.at(0,2), + mRcw.at(1,0), mRcw.at(1,1), mRcw.at(1,2), + mRcw.at(2,0), mRcw.at(2,1), mRcw.at(2,2)); + mtcwx = cv::Matx31f(mtcw.at(0), mtcw.at(1), mtcw.at(2)); + } cv::Mat Frame::GetImuPosition() @@ -579,17 +612,17 @@ bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) // 3D in absolute coordinates // Step 1 获得这个地图点的世界坐标 - cv::Mat P = pMP->GetWorldPos(); + cv::Matx31f Px = pMP->GetWorldPos2(); // cout << "b"; // 3D in camera coordinates // 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc - const cv::Mat Pc = mRcw*P+mtcw; + const cv::Matx31f Pc = mRcwx * Px + mtcwx; const float Pc_dist = cv::norm(Pc); // Check positive depth - const float &PcZ = Pc.at(2); + const float &PcZ = Pc(2); const float invz = 1.0f/PcZ; // Step 2 关卡一：检查这个地图点在当前帧的相机坐标系下，是否有正的深度.如果是负的，表示出错，直接返回false if(PcZ<0.0f) @@ -615,7 +648,7 @@ bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) const float minDistance = pMP->GetMinDistanceInvariance(); // 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以 // mOw：当前相机光心在世界坐标系下坐标 - const cv::Mat PO = P-mOw; + const cv::Matx31f PO = Px-mOwx; //取模就得到了距离 const float dist = cv::norm(PO); @@ -627,11 +660,11 @@ bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) // Check viewing angle // Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, 若小于cos(viewingCosLimit), 即夹角大于viewingCosLimit弧度则返回 - cv::Mat Pn = pMP->GetNormal(); + cv::Matx31f Pnx = pMP->GetNormal2(); // cout << "f"; // 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量 - const float viewCos = PO.dot(Pn)/dist; + const float viewCos = PO.dot(Pnx)/dist; //如果大于给定的阈值 cos(60°)=0.5，认为这个点方向太偏了，重投影不可靠，返回false if(viewCosmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1); - //IL.convertTo(IL,CV_32F); - // 图像块均值归一化，降低亮度变化对相似度计算的影响 - //IL = IL - IL.at(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F); - IL.convertTo(IL,CV_16S); - IL = IL - IL.at(w,w); + //初始化最佳相似度 int bestDist = INT_MAX; // 通过滑动窗口搜索优化，得到的列坐标偏移量 @@ -1163,11 +1192,7 @@ void Frame::ComputeStereoMatches() { // 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1); - //IR.convertTo(IR,CV_32F); - // 图像块均值归一化，降低亮度变化对相似度计算的影响 - //IR = IR - IR.at(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F); - IR.convertTo(IR,CV_16S); - IR = IR - IR.at(w,w); + // sad 计算 float dist = cv::norm(IL,IR,cv::NORM_L1); // 统计最小sad和偏移量 @@ -1316,10 +1341,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt :mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), mTlr(Tlr) { - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); imgLeft = imLeft.clone(); imgRight = imRight.clone(); - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); // Frame ID mnId=nNextId++; @@ -1334,11 +1357,18 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); // ORB extraction +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); +#endif thread threadLeft(&Frame::ExtractORB,this,0,imLeft,static_cast(mpCamera)->mvLappingArea[0],static_cast(mpCamera)->mvLappingArea[1]); thread threadRight(&Frame::ExtractORB,this,1,imRight,static_cast(mpCamera2)->mvLappingArea[0],static_cast(mpCamera2)->mvLappingArea[1]); threadLeft.join(); threadRight.join(); - std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); + + mTimeORB_Ext = std::chrono::duration_cast >(time_EndExtORB - time_StartExtORB).count(); +#endif Nleft = mvKeys.size(); Nright = mvKeysRight.size(); @@ -1375,8 +1405,22 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt cv::hconcat(Rrl,trl,mTrl); + mTrlx = cv::Matx34f(Rrl.at(0,0), Rrl.at(0,1), Rrl.at(0,2), trl.at(0), + Rrl.at(1,0), Rrl.at(1,1), Rrl.at(1,2), trl.at(1), + Rrl.at(2,0), Rrl.at(2,1), Rrl.at(2,2), trl.at(2)); + mTlrx = cv::Matx34f(mRlr.at(0,0), mRlr.at(0,1), mRlr.at(0,2), mtlr.at(0), + mRlr.at(1,0), mRlr.at(1,1), mRlr.at(1,2), mtlr.at(1), + mRlr.at(2,0), mRlr.at(2,1), mRlr.at(2,2), mtlr.at(2)); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now(); +#endif ComputeStereoFishEyeMatches(); - std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now(); + + mTimeStereoMatch = std::chrono::duration_cast >(time_EndStereoMatches - time_StartStereoMatches).count(); +#endif //Put all descriptors in the same matrix cv::vconcat(mDescriptors,mDescriptorsRight,mDescriptors); @@ -1385,25 +1429,10 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt mvbOutlier = vector(N,false); AssignFeaturesToGrid(); - std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now(); mpMutexImu = new std::mutex(); UndistortKeyPoints(); - std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now(); - - double t_read = std::chrono::duration_cast >(t1 - t0).count(); - double t_orbextract = std::chrono::duration_cast >(t2 - t1).count(); - double t_stereomatches = std::chrono::duration_cast >(t3 - t2).count(); - double t_assign = std::chrono::duration_cast >(t4 - t3).count(); - double t_undistort = std::chrono::duration_cast >(t5 - t4).count(); - - /*cout << "Reading time: " << t_read << endl; - cout << "Extraction time: " << t_orbextract << endl; - cout << "Matching time: " << t_stereomatches << endl; - cout << "Assignment time: " << t_assign << endl; - cout << "Undistortion time: " << t_undistort << endl;*/ - } void Frame::ComputeStereoFishEyeMatches() { @@ -1450,26 +1479,33 @@ void Frame::ComputeStereoFishEyeMatches() { bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) { // 3D in absolute coordinates - cv::Mat P = pMP->GetWorldPos(); + cv::Matx31f Px = pMP->GetWorldPos2(); + + cv::Matx33f mRx; + cv::Matx31f mtx, twcx; + + cv::Matx33f Rcw = mRcwx; + cv::Matx33f Rwc = mRcwx.t(); + cv::Matx31f tcw = mOwx; - cv::Mat mR, mt, twc; if(bRight){ - cv::Mat Rrl = mTrl.colRange(0,3).rowRange(0,3); - cv::Mat trl = mTrl.col(3); - mR = Rrl * mRcw; - mt = Rrl * mtcw + trl; - twc = mRwc * mTlr.rowRange(0,3).col(3) + mOw; + cv::Matx33f Rrl = mTrlx.get_minor<3,3>(0,0); + cv::Matx31f trl = mTrlx.get_minor<3,1>(0,3); + mRx = Rrl * Rcw; + mtx = Rrl * tcw + trl; + twcx = Rwc * mTlrx.get_minor<3,1>(0,3) + tcw; } else{ - mR = mRcw; - mt = mtcw; - twc = mOw; + mRx = mRcwx; + mtx = mtcwx; + twcx = mOwx; } // 3D in camera coordinates - cv::Mat Pc = mR*P+mt; - const float Pc_dist = cv::norm(Pc); - const float &PcZ = Pc.at(2); + + cv::Matx31f Pcx = mRx * Px + mtx; + const float Pc_dist = cv::norm(Pcx); + const float &PcZ = Pcx(2); // Check positive depth if(PcZ<0.0f) @@ -1477,8 +1513,8 @@ bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) // Project in image and check it is not outside cv::Point2f uv; - if(bRight) uv = mpCamera2->project(Pc); - else uv = mpCamera->project(Pc); + if(bRight) uv = mpCamera2->project(Pcx); + else uv = mpCamera->project(Pcx); if(uv.xmnMaxX) return false; @@ -1488,16 +1524,16 @@ bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) // Check distance is in the scale invariance region of the MapPoint const float maxDistance = pMP->GetMaxDistanceInvariance(); const float minDistance = pMP->GetMinDistanceInvariance(); - const cv::Mat PO = P-twc; - const float dist = cv::norm(PO); + const cv::Matx31f POx = Px - twcx; + const float dist = cv::norm(POx); if(distmaxDistance) return false; // Check viewing angle - cv::Mat Pn = pMP->GetNormal(); + cv::Matx31f Pnx = pMP->GetNormal2(); - const float viewCos = PO.dot(Pn)/dist; + const float viewCos = POx.dot(Pnx)/dist; if(viewCos vIniKeys; // Initialization: KeyPoints in reference frame vector vMatches; // Initialization: correspondeces with reference keypoints @@ -47,7 +46,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) vector > vTracks; int state; // Tracking state - // Frame currentFrame; vector vpLocalMap; vector vMatchesKeys; @@ -73,7 +71,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) vMatches = mvIniMatches; vTracks = mvTracks; } - else if(mState==Tracking::OK /*&& bOldFeatures*/) + else if(mState==Tracking::OK) { vCurrentKeys = mvCurrentKeys; vbVO = mvbVO; @@ -96,7 +94,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) } if(im.channels()<3) //this should be always true - cvtColor(im,im,CV_GRAY2BGR); + cvtColor(im,im,cv::COLOR_GRAY2BGR); //Draw if(state==Tracking::NOT_INITIALIZED) @@ -143,12 +141,8 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) mnTrackedVO++; } } - /*else - { - cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(0,0,255),-1); - }*/ + } - // std::cout << "2.3" << std::endl; } else if(state==Tracking::OK && !bOldFeatures) { @@ -157,9 +151,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) mnTrackedVO=0; int n = vCurrentKeys.size(); - // cout << "----------------------" << endl; - // cout << "Number of matches in old method: " << n << endl; - for(int i=0; i < n; ++i) { @@ -170,10 +161,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) } } - n = mProjectPoints.size(); - //cout << "Number of projected points: " << n << endl; - n = mMatchedInImage.size(); - //cout << "Number of matched points: " << n << endl; map::iterator it_match = mMatchedInImage.begin(); while(it_match != mMatchedInImage.end()) { @@ -193,37 +180,9 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) it_match++; - //it_proj = mProjectPoints.erase(it_proj); } - //for(int i=0; i < n; ++i) - //{ - /*if(!vpMatchedMPs[i]) - continue;*/ - - //cv::circle(im,vProjectPoints[i],2,cv::Scalar(255,0,0),-1); - /*cv::Point2f point3d_proy; - float u, v; - bool bIsInImage = currentFrame.ProjectPointDistort(vpMatchedMPs[i] , point3d_proy, u, v); - if(bIsInImage) - { - //cout << "-Point is out of the image" << point3d_proy.x << ", " << point3d_proy.y << endl; - cv::circle(im,vMatchesKeys[i].pt,2,cv::Scalar(255,0,0),-1); - continue; - } - - //cout << "+Point CV " << point3d_proy.x << ", " << point3d_proy.y << endl; - //cout << "+Point coord " << u << ", " << v << endl; - cv::Point2f point_im = vMatchesKeys[i].pt; - - cv::line(im,cv::Point2f(u, v), point_im,cv::Scalar(0, 255, 0), 1);*/ - - //} - - /*cout << "Number of tracker in old method: " << mnTracked << endl; - cout << "Number of tracker in new method: " << nTracked2 << endl;*/ n = vOutlierKeys.size(); - //cout << "Number of outliers: " << n << endl; for(int i=0; i < n; ++i) { cv::Point2f point3d_proy; @@ -235,34 +194,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures) cv::line(im,cv::Point2f(u, v), point_im,cv::Scalar(0, 0, 255), 1); } -// for(int i=0;i release mutex if(im.channels()<3) //this should be always true - cvtColor(im,im,CV_GRAY2BGR); + cvtColor(im,im,cv::COLOR_GRAY2BGR); //Draw if(state==Tracking::NOT_INITIALIZED) //INITIALIZING @@ -418,8 +350,6 @@ void FrameDrawer::Update(Tracking *pTracker) N = mvCurrentKeys.size(); } - //cout << "Number of matches in frame: " << N << endl; - // cout << "Number of matches in frame: " << N << endl; mvbVO = vector(N,false); mvbMap = vector(N,false); mbOnlyTracking = pTracker->mbOnlyTracking; @@ -427,7 +357,6 @@ void FrameDrawer::Update(Tracking *pTracker) //Variables for the new visualization mCurrentFrame = pTracker->mCurrentFrame; mmProjectPoints = mCurrentFrame.mmProjectPoints; - //mmMatchedInImage = mCurrentFrame.mmMatchedInImage; mmMatchedInImage.clear(); mvpLocalMap = pTracker->GetLocalMapMPS(); @@ -439,8 +368,6 @@ void FrameDrawer::Update(Tracking *pTracker) mvOutlierKeys.reserve(N); mvpOutlierMPs.clear(); mvpOutlierMPs.reserve(N); - //mvProjectPoints.clear(); - //mvProjectPoints.reserve(N); if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED) { @@ -461,18 +388,8 @@ void FrameDrawer::Update(Tracking *pTracker) else mvbVO[i]=true; - //mvpMatchedMPs.push_back(pMP); - //mvMatchedKeys.push_back(mvCurrentKeys[i]); mmMatchedInImage[pMP->mnId] = mvCurrentKeys[i].pt; - //cv::Point2f point3d_proy; - //float u, v; - //bool bIsInImage = mCurrentFrame.ProjectPointDistort(pMP, point3d_proy, u, v); - //if(bIsInImage) - //{ - //mvMatchedKeys.push_back(mvCurrentKeys[i]); - //mvProjectPoints.push_back(cv::Point2f(u, v)); - //} } else { diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc index c1ad448..e416106 100644 --- a/src/G2oTypes.cc +++ b/src/G2oTypes.cc @@ -424,61 +424,6 @@ void EdgeStereo::linearizeOplus() y , -x , 0.0, 0.0, 0.0, 1.0; _jacobianOplusXj = proj_jac * Rcb * SE3deriv; - - - /*const VertexPose* VPose = static_cast(_vertices[1]); - const g2o::VertexSBAPointXYZ* VPoint = static_cast(_vertices[0]); - - const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw; - const Eigen::Vector3d &tcw = VPose->estimate().tcw; - const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw; - const double &xc = Xc[0]; - const double &yc = Xc[1]; - const double invzc = 1.0/Xc[2]; - const double invzc2 = invzc*invzc; - const double &fx = VPose->estimate().fx; - const double &fy = VPose->estimate().fy; - const double &bf = VPose->estimate().bf; - const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb; - - // Jacobian wrt Point - _jacobianOplusXi(0,0) = -fx*invzc*Rcw(0,0)+fx*xc*invzc2*Rcw(2,0); - _jacobianOplusXi(0,1) = -fx*invzc*Rcw(0,1)+fx*xc*invzc2*Rcw(2,1); - _jacobianOplusXi(0,2) = -fx*invzc*Rcw(0,2)+fx*xc*invzc2*Rcw(2,2); - - _jacobianOplusXi(1,0) = -fy*invzc*Rcw(1,0)+fy*yc*invzc2*Rcw(2,0); - _jacobianOplusXi(1,1) = -fy*invzc*Rcw(1,1)+fy*yc*invzc2*Rcw(2,1); - _jacobianOplusXi(1,2) = -fy*invzc*Rcw(1,2)+fy*yc*invzc2*Rcw(2,2); - - _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*invzc2*Rcw(2,0); - _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*invzc2*Rcw(2,1); - _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*invzc2*Rcw(2,2); - - const Eigen::Vector3d Xb = VPose->estimate().Rbc*Xc + VPose->estimate().tbc; - const Eigen::Matrix3d RS = VPose->estimate().Rcb*Skew(Xb); - - // Jacobian wrt Imu Pose - _jacobianOplusXj(0,0) = -fx*invzc*RS(0,0)+fx*xc*invzc2*RS(2,0); - _jacobianOplusXj(0,1) = -fx*invzc*RS(0,1)+fx*xc*invzc2*RS(2,1); - _jacobianOplusXj(0,2) = -fx*invzc*RS(0,2)+fx*xc*invzc2*RS(2,2); - _jacobianOplusXj(0,3) = fx*invzc*Rcb(0,0)-fx*xc*invzc2*Rcb(2,0); - _jacobianOplusXj(0,4) = fx*invzc*Rcb(0,1)-fx*xc*invzc2*Rcb(2,1); - _jacobianOplusXj(0,5) = fx*invzc*Rcb(0,2)-fx*xc*invzc2*Rcb(2,2); - - _jacobianOplusXj(1,0) = -fy*invzc*RS(1,0)+fy*yc*invzc2*RS(2,0); - _jacobianOplusXj(1,1) = -fy*invzc*RS(1,1)+fy*yc*invzc2*RS(2,1); - _jacobianOplusXj(1,2) = -fy*invzc*RS(1,2)+fy*yc*invzc2*RS(2,2); - _jacobianOplusXj(1,3) = fy*invzc*Rcb(1,0)-fy*yc*invzc2*Rcb(2,0); - _jacobianOplusXj(1,4) = fy*invzc*Rcb(1,1)-fy*yc*invzc2*Rcb(2,1); - _jacobianOplusXj(1,5) = fy*invzc*Rcb(1,2)-fy*yc*invzc2*Rcb(2,2); - - _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0) - bf*invzc2*RS(2,0); - _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1) - bf*invzc2*RS(2,1); - _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2) - bf*invzc2*RS(2,2); - _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3) + bf*invzc2*Rcb(2,0); - _jacobianOplusXj(2,4) = _jacobianOplusXj(0,4) + bf*invzc2*Rcb(2,1); - _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5) + bf*invzc2*Rcb(2,2); - */ } void EdgeStereoOnlyPose::linearizeOplus() @@ -506,156 +451,8 @@ void EdgeStereoOnlyPose::linearizeOplus() -z , 0.0, x, 0.0, 1.0, 0.0, y , -x , 0.0, 0.0, 0.0, 1.0; _jacobianOplusXi = proj_jac * Rcb * SE3deriv; - - /*const VertexPose* VPose = static_cast(_vertices[0]); - const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw; - const Eigen::Vector3d &tcw = VPose->estimate().tcw; - const Eigen::Vector3d Xc = Rcw*Xw + tcw; - const double &xc = Xc[0]; - const double &yc = Xc[1]; - const double invzc = 1.0/Xc[2]; - const double invzc2 = invzc*invzc; - const double &fx = VPose->estimate().fx; - const double &fy = VPose->estimate().fy; - const double &bf = VPose->estimate().bf; - const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb; - - const Eigen::Vector3d Xb = VPose->estimate().Rbc*Xc + VPose->estimate().tbc; - const Eigen::Matrix3d RS = VPose->estimate().Rcb*Skew(Xb); - - // Jacobian wrt Imu Pose - _jacobianOplusXi(0,0) = -fx*invzc*RS(0,0)+fx*xc*invzc2*RS(2,0); - _jacobianOplusXi(0,1) = -fx*invzc*RS(0,1)+fx*xc*invzc2*RS(2,1); - _jacobianOplusXi(0,2) = -fx*invzc*RS(0,2)+fx*xc*invzc2*RS(2,2); - _jacobianOplusXi(0,3) = fx*invzc*Rcb(0,0)-fx*xc*invzc2*Rcb(2,0); - _jacobianOplusXi(0,4) = fx*invzc*Rcb(0,1)-fx*xc*invzc2*Rcb(2,1); - _jacobianOplusXi(0,5) = fx*invzc*Rcb(0,2)-fx*xc*invzc2*Rcb(2,2); - - _jacobianOplusXi(1,0) = -fy*invzc*RS(1,0)+fy*yc*invzc2*RS(2,0); - _jacobianOplusXi(1,1) = -fy*invzc*RS(1,1)+fy*yc*invzc2*RS(2,1); - _jacobianOplusXi(1,2) = -fy*invzc*RS(1,2)+fy*yc*invzc2*RS(2,2); - _jacobianOplusXi(1,3) = fy*invzc*Rcb(1,0)-fy*yc*invzc2*Rcb(2,0); - _jacobianOplusXi(1,4) = fy*invzc*Rcb(1,1)-fy*yc*invzc2*Rcb(2,1); - _jacobianOplusXi(1,5) = fy*invzc*Rcb(1,2)-fy*yc*invzc2*Rcb(2,2); - - _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0) - bf*invzc2*RS(2,0); - _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1) - bf*invzc2*RS(2,1); - _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2) - bf*invzc2*RS(2,2); - _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3) + bf*invzc2*Rcb(2,0); - _jacobianOplusXi(2,4) = _jacobianOplusXi(0,4) + bf*invzc2*Rcb(2,1); - _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5) + bf*invzc2*Rcb(2,2); - */ } -/*Eigen::Vector2d EdgeMonoInvdepth::cam_project(const Eigen::Vector3d & trans_xyz) const{ - double invZ = 1./trans_xyz[2]; - Eigen::Vector2d res; - res[0] = invZ*trans_xyz[0]*fx + cx; - res[1] = invZ*trans_xyz[1]*fy + cy; - return res; -} - -Eigen::Vector3d EdgeMonoInvdepth::cam_unproject(const double u, const double v, const double invDepth) const{ - Eigen::Vector3d res; - res[2] = 1./invDepth; - double z_x=res[2]/fx; - double z_y=res[2]/fy; - res[0] = (u-cx)*z_x; - res[1] = (v-cy)*z_y; - - return res; -} - -void EdgeMonoInvdepth::linearizeOplus() -{ - VertexInvDepth *vPt = static_cast(_vertices[0]); - g2o::VertexSE3Expmap *vHost = static_cast(_vertices[1]); - g2o::VertexSE3Expmap *vObs = static_cast(_vertices[2]); - - // - g2o::SE3Quat Tiw(vObs->estimate()); - g2o::SE3Quat T0w(vHost->estimate()); - g2o::SE3Quat Ti0 = Tiw*T0w.inverse(); - double o_rho_j = vPt->estimate().rho; - Eigen::Vector3d o_X_j = cam_unproject(vPt->estimate().u,vPt->estimate().v,o_rho_j); - Eigen::Vector3d i_X_j = Ti0.map(o_X_j); - double i_rho_j = 1./i_X_j[2]; - Eigen::Vector2d i_proj_j = cam_project(i_X_j); - - // i_rho_j*C_ij matrix - Eigen::Matrix rhoC_ij; - rhoC_ij(0,0) = i_rho_j*fx; - rhoC_ij(0,1) = 0.0; - rhoC_ij(0,2) = i_rho_j*(cx-i_proj_j[0]); - rhoC_ij(1,0) = 0.0; - rhoC_ij(1,1) = i_rho_j*fy; - rhoC_ij(1,2) = i_rho_j*(cy-i_proj_j[1]); - - // o_rho_j^{-2}*K^{-1}*0_proj_j vector - Eigen::Matrix3d Ri0 = Ti0.rotation().toRotationMatrix(); - Eigen::Matrix tmp; - tmp = rhoC_ij*Ri0; - - // Skew matrices - Eigen::Matrix3d skew_0_X_j; - Eigen::Matrix3d skew_i_X_j; - - skew_0_X_j=Eigen::MatrixXd::Zero(3,3); - skew_i_X_j=Eigen::MatrixXd::Zero(3,3); - - skew_0_X_j(0,1) = -o_X_j[2]; - skew_0_X_j(1,0) = -skew_0_X_j(0,1); - skew_0_X_j(0,2) = o_X_j[1]; - skew_0_X_j(2,0) = -skew_0_X_j(0,2); - skew_0_X_j(1,2) = -o_X_j[0]; - skew_0_X_j(2,1) = -skew_0_X_j(1,2); - - skew_i_X_j(0,1) = -i_X_j[2]; - skew_i_X_j(1,0) = -skew_i_X_j(0,1); - skew_i_X_j(0,2) = i_X_j[1]; - skew_i_X_j(2,0) = -skew_i_X_j(0,2); - skew_i_X_j(1,2) = -i_X_j[0]; - skew_i_X_j(2,1) = -skew_i_X_j(1,2); - - // Jacobians w.r.t inverse depth in the host frame - _jacobianOplus[0].setZero(); - _jacobianOplus[0] = (1./o_rho_j)*rhoC_ij*Ri0*o_X_j; - - // Jacobians w.r.t pose host frame - _jacobianOplus[1].setZero(); - // Rotation - _jacobianOplus[1].block<2,3>(0,0) = -tmp*skew_0_X_j; - // translation - _jacobianOplus[1].block<2,3>(0,3) = tmp; - - // Jacobians w.r.t pose observer frame - _jacobianOplus[2].setZero(); - // Rotation - _jacobianOplus[2].block<2,3>(0,0) = rhoC_ij*skew_i_X_j; - // translation - _jacobianOplus[2].block<2,3>(0,3) = -rhoC_ij; -} - - -Eigen::Vector2d EdgeMonoInvdepthBody::cam_project(const Eigen::Vector3d & trans_xyz) const{ - double invZ = 1./trans_xyz[2]; - Eigen::Vector2d res; - res[0] = invZ*trans_xyz[0]*fx + cx; - res[1] = invZ*trans_xyz[1]*fy + cy; - return res; -} - -Eigen::Vector3d EdgeMonoInvdepthBody::cam_unproject(const double u, const double v, const double invDepth) const{ - Eigen::Vector3d res; - res[2] = 1./invDepth; - double z_x=res[2]/fx; - double z_y=res[2]/fy; - res[0] = (u-cx)*z_x; - res[1] = (v-cy)*z_y; - - return res; -}*/ - VertexVelocity::VertexVelocity(KeyFrame* pKF) { setEstimate(Converter::toVector3d(pKF->GetVelocity())); diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index 2df1a32..d5a4457 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -31,7 +31,6 @@ cv::Mat NormalizeRotation(const cv::Mat &R) { cv::Mat U,w,Vt; cv::SVDecomp(R,w,U,Vt,cv::SVD::FULL_UV); - // assert(cv::determinant(U*Vt)>0); return U*Vt; } @@ -399,10 +398,10 @@ IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_) cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_) { std::unique_lock lock(mMutex); - // ? 计算偏置的变化量 + // 计算偏置的变化量 cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); - // ? 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14 - // ? Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的 + // 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14 + // Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的 return NormalizeRotation(dR*ExpSO3(JRg*dbg)); } @@ -411,7 +410,7 @@ cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_) std::unique_lock lock(mMutex); cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); cv::Mat dba = (cv::Mat_(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz); - // ? 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 + // 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dV + JVg*dbg + JVa*dba; } @@ -420,7 +419,7 @@ cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_) std::unique_lock lock(mMutex); cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); cv::Mat dba = (cv::Mat_(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz); - // ? 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 + // 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dP + JPg*dbg + JPa*dba; } diff --git a/src/Initializer.cc b/src/Initializer.cc index 737c6e2..8665f1e 100644 --- a/src/Initializer.cc +++ b/src/Initializer.cc @@ -196,11 +196,11 @@ bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatc //更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果 return ReconstructH(vbMatchesInliersH, //输入，匹配成功的特征点对Inliers标记 H, //输入，前面RANSAC计算后的单应矩阵 - mK, //输入，相机的内参数矩阵 + K, //输入，相机的内参数矩阵 R21,t21, //输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换 vP3D, //特征点对经过三角测量之后的空间坐标，也就是地图点 vbTriangulated, //特征点对是否成功三角化的标记 - 1.0, //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时 + minParallax, //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时 //需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度 50); //为了进行运动恢复，所需要的最少的三角化测量成功的点个数 } diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 10347e2..b9df0f7 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -96,6 +96,12 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): SetPose(F.mTcw); mnOriginMapId = pMap->GetId(); + + this->Tlr_ = cv::Matx44f(mTlr.at(0,0),mTlr.at(0,1),mTlr.at(0,2),mTlr.at(0,3), + mTlr.at(1,0),mTlr.at(1,1),mTlr.at(1,2),mTlr.at(1,3), + mTlr.at(2,0),mTlr.at(2,1),mTlr.at(2,2),mTlr.at(2,3), + mTlr.at(3,0),mTlr.at(3,1),mTlr.at(3,2),mTlr.at(3,3)); + } void KeyFrame::ComputeBoW() @@ -133,6 +139,19 @@ void KeyFrame::SetPose(const cv::Mat &Tcw_) cv::Mat center = (cv::Mat_(4,1) << mHalfBaseline, 0 , 0, 1); // 世界坐标系下，左目相机中心到立体相机中心的向量，方向由左目相机指向立体相机中心 Cw = Twc*center; + + //Static matrices + this->Tcw_ = cv::Matx44f(Tcw.at(0,0),Tcw.at(0,1),Tcw.at(0,2),Tcw.at(0,3), + Tcw.at(1,0),Tcw.at(1,1),Tcw.at(1,2),Tcw.at(1,3), + Tcw.at(2,0),Tcw.at(2,1),Tcw.at(2,2),Tcw.at(2,3), + Tcw.at(3,0),Tcw.at(3,1),Tcw.at(3,2),Tcw.at(3,3)); + + this->Twc_ = cv::Matx44f(Twc.at(0,0),Twc.at(0,1),Twc.at(0,2),Twc.at(0,3), + Twc.at(1,0),Twc.at(1,1),Twc.at(1,2),Twc.at(1,3), + Twc.at(2,0),Twc.at(2,1),Twc.at(2,2),Twc.at(2,3), + Twc.at(3,0),Twc.at(3,1),Twc.at(3,2),Twc.at(3,3)); + + this->Ow_ = cv::Matx31f(Ow.at(0),Ow.at(1),Ow.at(2)); } void KeyFrame::SetVelocity(const cv::Mat &Vw_) @@ -248,9 +267,12 @@ void KeyFrame::UpdateBestCovisibles() list lWs; // weight for(size_t i=0, iend=vPairs.size(); iisBad()) + { + // push_front 后变成从大到小 + lKFs.push_front(vPairs[i].second); + lWs.push_front(vPairs[i].first); + } } // 权重从大到小 @@ -739,11 +761,6 @@ void KeyFrame::SetBadFlag() mbToBeErased = true; return; } - if(!mpParent) - { - //cout << "KF.BADFLAG-> There is not parent, but it is not the first KF in the map" << endl; - //cout << "KF.BADFLAG-> KF: " << mnId << "; first KF: " << mpMap->GetInitKFid() << endl; - } } // Step 2 遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系 @@ -830,12 +847,6 @@ void KeyFrame::SetBadFlag() // 回答：不需要！这里while循环还没退出，会使用更新的sParentCandidates if(bContinue) { - if(pC->mnId == pP->mnId) - { - /*cout << "ERROR: The parent and son can't be the same KF. ID: " << pC->mnId << endl; - cout << "Current KF: " << mnId << endl; - cout << "Parent of the map: " << endl;*/ - } // 因为父节点死了，并且子节点找到了新的父节点，就把它更新为自己的父节点 pC->ChangeParent(pP); // 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点 @@ -865,10 +876,6 @@ void KeyFrame::SetBadFlag() // 如果当前的关键帧要被删除的话就要计算这个,表示原父关键帧到当前关键帧的位姿变换 // 注意在这个删除的过程中,其实并没有将当前关键帧中存储的父关键帧的指针删除掉 mTcp = Tcw*mpParent->GetPoseInverse(); - } - else - { - //cout << "Error: KF haven't got a parent, it is imposible reach this code point without him" << endl; } // 标记当前关键帧已经死了 mbBad = true; @@ -1062,171 +1069,6 @@ void KeyFrame::UpdateMap(Map* pMap) mpMap = pMap; } -void KeyFrame::PreSave(set& spKF,set& spMP, set& spCam) -{ - // Save the id of each MapPoint in this KF, there can be null pointer in the vector - mvBackupMapPointsId.clear(); - mvBackupMapPointsId.reserve(N); - for(int i = 0; i < N; ++i) - { - - if(mvpMapPoints[i] && spMP.find(mvpMapPoints[i]) != spMP.end()) // Checks if the element is not null - mvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId); - else // If the element is null his value is -1 because all the id are positives - mvBackupMapPointsId.push_back(-1); - } - //cout << "KeyFrame: ID from MapPoints stored" << endl; - // Save the id of each connected KF with it weight - mBackupConnectedKeyFrameIdWeights.clear(); - for(std::map::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it) - { - if(spKF.find(it->first) != spKF.end()) - mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second; - } - //cout << "KeyFrame: ID from connected KFs stored" << endl; - // Save the parent id - mBackupParentId = -1; - if(mpParent && spKF.find(mpParent) != spKF.end()) - mBackupParentId = mpParent->mnId; - //cout << "KeyFrame: ID from Parent KF stored" << endl; - // Save the id of the childrens KF - mvBackupChildrensId.clear(); - mvBackupChildrensId.reserve(mspChildrens.size()); - for(KeyFrame* pKFi : mspChildrens) - { - if(spKF.find(pKFi) != spKF.end()) - mvBackupChildrensId.push_back(pKFi->mnId); - } - //cout << "KeyFrame: ID from Children KFs stored" << endl; - // Save the id of the loop edge KF - mvBackupLoopEdgesId.clear(); - mvBackupLoopEdgesId.reserve(mspLoopEdges.size()); - for(KeyFrame* pKFi : mspLoopEdges) - { - if(spKF.find(pKFi) != spKF.end()) - mvBackupLoopEdgesId.push_back(pKFi->mnId); - } - //cout << "KeyFrame: ID from Loop KFs stored" << endl; - // Save the id of the merge edge KF - mvBackupMergeEdgesId.clear(); - mvBackupMergeEdgesId.reserve(mspMergeEdges.size()); - for(KeyFrame* pKFi : mspMergeEdges) - { - if(spKF.find(pKFi) != spKF.end()) - mvBackupMergeEdgesId.push_back(pKFi->mnId); - } - //cout << "KeyFrame: ID from Merge KFs stored" << endl; - - //Camera data - mnBackupIdCamera = -1; - if(mpCamera && spCam.find(mpCamera) != spCam.end()) - mnBackupIdCamera = mpCamera->GetId(); - //cout << "KeyFrame: ID from Camera1 stored; " << mnBackupIdCamera << endl; - - mnBackupIdCamera2 = -1; - if(mpCamera2 && spCam.find(mpCamera2) != spCam.end()) - mnBackupIdCamera2 = mpCamera2->GetId(); - //cout << "KeyFrame: ID from Camera2 stored; " << mnBackupIdCamera2 << endl; - - //Inertial data - mBackupPrevKFId = -1; - if(mPrevKF && spKF.find(mPrevKF) != spKF.end()) - mBackupPrevKFId = mPrevKF->mnId; - //cout << "KeyFrame: ID from Prev KF stored" << endl; - mBackupNextKFId = -1; - if(mNextKF && spKF.find(mNextKF) != spKF.end()) - mBackupNextKFId = mNextKF->mnId; - //cout << "KeyFrame: ID from NextKF stored" << endl; - if(mpImuPreintegrated) - mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated); - //cout << "KeyFrame: Imu Preintegrated stored" << endl; -} - -void KeyFrame::PostLoad(map& mpKFid, map& mpMPid, map& mpCamId){ - // Rebuild the empty variables - - // Pose - SetPose(Tcw); - - // Reference reconstruction - // Each MapPoint sight from this KeyFrame - mvpMapPoints.clear(); - mvpMapPoints.resize(N); - for(int i=0; i(NULL); - } - - // Conected KeyFrames with him weight - mConnectedKeyFrameWeights.clear(); - for(map::const_iterator it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end(); - it != end; ++it) - { - KeyFrame* pKFi = mpKFid[it->first]; - mConnectedKeyFrameWeights[pKFi] = it->second; - } - - // Restore parent KeyFrame - if(mBackupParentId>=0) - mpParent = mpKFid[mBackupParentId]; - - // KeyFrame childrens - mspChildrens.clear(); - for(vector::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it!=end; ++it) - { - mspChildrens.insert(mpKFid[*it]); - } - - // Loop edge KeyFrame - mspLoopEdges.clear(); - for(vector::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it) - { - mspLoopEdges.insert(mpKFid[*it]); - } - - // Merge edge KeyFrame - mspMergeEdges.clear(); - for(vector::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it) - { - mspMergeEdges.insert(mpKFid[*it]); - } - - //Camera data - if(mnBackupIdCamera >= 0) - { - mpCamera = mpCamId[mnBackupIdCamera]; - } - if(mnBackupIdCamera2 >= 0) - { - mpCamera2 = mpCamId[mnBackupIdCamera2]; - } - - //Inertial data - if(mBackupPrevKFId != -1) - { - mPrevKF = mpKFid[mBackupPrevKFId]; - } - if(mBackupNextKFId != -1) - { - mNextKF = mpKFid[mBackupNextKFId]; - } - mpImuPreintegrated = &mBackupImuPreintegrated; - - - // Remove all backup container - mvBackupMapPointsId.clear(); - mBackupConnectedKeyFrameIdWeights.clear(); - mvBackupChildrensId.clear(); - mvBackupLoopEdgesId.clear(); - - UpdateBestCovisibles(); - - //ComputeSceneMedianDepth(); -} - bool KeyFrame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v) { @@ -1427,4 +1269,94 @@ void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase* pKFDB) mpKeyFrameDB = pKFDB; } +cv::Matx33f KeyFrame::GetRotation_() { + unique_lock lock(mMutexPose); + return Tcw_.get_minor<3,3>(0,0); +} + +cv::Matx31f KeyFrame::GetTranslation_() { + unique_lock lock(mMutexPose); + return Tcw_.get_minor<3,1>(0,3); +} + +cv::Matx31f KeyFrame::GetCameraCenter_() { + unique_lock lock(mMutexPose); + return Ow_; +} + +cv::Matx33f KeyFrame::GetRightRotation_() { + unique_lock lock(mMutexPose); + cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t(); + cv::Matx33f Rlw = Tcw_.get_minor<3,3>(0,0); + cv::Matx33f Rrw = Rrl * Rlw; + + return Rrw; +} + +cv::Matx31f KeyFrame::GetRightTranslation_() { + unique_lock lock(mMutexPose); + cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t(); + cv::Matx31f tlw = Tcw_.get_minor<3,1>(0,3); + cv::Matx31f trl = - Rrl * Tlr_.get_minor<3,1>(0,3); + + cv::Matx31f trw = Rrl * tlw + trl; + + return trw; +} + +cv::Matx44f KeyFrame::GetRightPose_() { + unique_lock lock(mMutexPose); + + cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t(); + cv::Matx33f Rlw = Tcw_.get_minor<3,3>(0,0); + cv::Matx33f Rrw = Rrl * Rlw; + + cv::Matx31f tlw = Tcw_.get_minor<3,1>(0,3); + cv::Matx31f trl = - Rrl * Tlr_.get_minor<3,1>(0,3); + + cv::Matx31f trw = Rrl * tlw + trl; + + cv::Matx44f Trw{Rrw(0,0),Rrw(0,1),Rrw(0,2),trw(0), + Rrw(1,0),Rrw(1,1),Rrw(1,2),trw(1), + Rrw(2,0),Rrw(2,1),Rrw(2,2),trw(2), + 0.f,0.f,0.f,1.f}; + + return Trw; +} + +cv::Matx31f KeyFrame::GetRightCameraCenter_() { + unique_lock lock(mMutexPose); + cv::Matx33f Rwl = Tcw_.get_minor<3,3>(0,0).t(); + cv::Matx31f tlr = Tlr_.get_minor<3,1>(0,3); + + cv::Matx31f twr = Rwl * tlr + Ow_; + + return twr; +} + +cv::Matx31f KeyFrame::UnprojectStereo_(int i) { + const float z = mvDepth[i]; + if(z>0) + { + const float u = mvKeys[i].pt.x; + const float v = mvKeys[i].pt.y; + const float x = (u-cx)*z*invfx; + const float y = (v-cy)*z*invfy; + cv::Matx31f x3Dc(x,y,z); + + unique_lock lock(mMutexPose); + return Twc_.get_minor<3,3>(0,0) * x3Dc + Twc_.get_minor<3,1>(0,3); + } + else + return cv::Matx31f::zeros(); +} + +cv::Matx44f KeyFrame::GetPose_() +{ + unique_lock lock(mMutexPose); + return Tcw_; +} + + + } //namespace ORB_SLAM diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 50a46e5..babf5a8 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -934,36 +934,6 @@ vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map return vpRelocCandidates; } -void KeyFrameDatabase::PreSave() -{ - //Save the information about the inverted index of KF to node - mvBackupInvertedFileId.resize(mvInvertedFile.size()); - for(int i = 0, numEl = mvInvertedFile.size(); i < numEl; ++i) - { - for(std::list::const_iterator it = mvInvertedFile[i].begin(), end = mvInvertedFile[i].end(); it != end; ++it) - { - mvBackupInvertedFileId[i].push_back((*it)->mnId); - } - } -} - -void KeyFrameDatabase::PostLoad(map mpKFid) -{ - mvInvertedFile.clear(); - mvInvertedFile.resize(mpVoc->size()); - for(unsigned int i = 0; i < mvBackupInvertedFileId.size(); ++i) - { - for(long unsigned int KFid : mvBackupInvertedFileId[i]) - { - if(mpKFid.find(KFid) != mpKFid.end()) - { - mvInvertedFile[i].push_back(mpKFid[KFid]); - } - } - } - -} - void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc) { ORBVocabulary** ptr; diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index a216579..1548d01 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -22,6 +22,7 @@ #include "ORBmatcher.h" #include "Optimizer.h" #include "Converter.h" +#include "Config.h" #include #include @@ -32,7 +33,7 @@ namespace ORB_SLAM3 LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName): mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true), - mbNewInit(false), mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9,9)) + mbNewInit(false), mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), infoInertial(Eigen::MatrixXd::Zero(9,9)) { /* * mbStopRequested： 外部线程调用，为true，表示外部线程请求停止 local mapping @@ -53,15 +54,11 @@ LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular, mNumLM = 0; mNumKFCulling=0; - //DEBUG: times and data from LocalMapping in each frame +#ifdef REGISTER_TIMES + nLBA_exec = 0; + nLBA_abort = 0; +#endif - strSequence = "";//_strSeqName; - - //f_lm.open("localMapping_times" + strSequence + ".txt"); - /*f_lm.open("localMapping_times.txt"); - - f_lm << "# Timestamp KF, Num CovKFs, Num KFs, Num RecentMPs, Num MPs, processKF, MPCulling, CreateMP, SearchNeigh, BA, KFCulling, [numFixKF_LBA]" << endl; - f_lm << fixed;*/ } // 设置回环检测线程句柄 @@ -94,31 +91,34 @@ void LocalMapping::Run() // 等待处理的关键帧列表不为空 并且imu正常 if(CheckNewKeyFrames() && !mbBadImu) { - // std::cout << "LM" << std::endl; - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + double timeLBA_ms = 0; + double timeKFCulling_ms = 0; + + std::chrono::steady_clock::time_point time_StartProcessKF = std::chrono::steady_clock::now(); +#endif // BoW conversion and insertion in Map // Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等 ProcessNewKeyFrame(); - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndProcessKF = std::chrono::steady_clock::now(); + double timeProcessKF = std::chrono::duration_cast >(time_EndProcessKF - time_StartProcessKF).count(); + vdKFInsert_ms.push_back(timeProcessKF); +#endif // Check recent MapPoints // Step 3 根据地图点的观测情况剔除质量不好的地图点 MapPointCulling(); - std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMPCulling = std::chrono::steady_clock::now(); + double timeMPCulling = std::chrono::duration_cast >(time_EndMPCulling - time_EndProcessKF).count(); + vdMPCulling_ms.push_back(timeMPCulling); +#endif // Triangulate new MapPoints // Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳 CreateNewMapPoints(); - std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); - - // Save here: - // # Cov KFs - // # tot Kfs - // # recent added MPs - // # tot MPs - // # localMPs in LBA - // # fixedKFs in LBA mbAbortBA = false; // 已经处理完队列中的最后的一个关键帧 @@ -129,20 +129,19 @@ void LocalMapping::Run() SearchInNeighbors(); } - std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point t5 = t4, t6 = t4; - // mbAbortBA = false; +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMPCreation = std::chrono::steady_clock::now(); - //DEBUG-- - /*f_lm << setprecision(0); - f_lm << mpCurrentKeyFrame->mTimeStamp*1e9 << ","; - f_lm << mpCurrentKeyFrame->GetVectorCovisibleKeyFrames().size() << ","; - f_lm << mpCurrentKeyFrame->GetMap()->GetAllKeyFrames().size() << ","; - f_lm << mlpRecentAddedMapPoints.size() << ","; - f_lm << mpCurrentKeyFrame->GetMap()->GetAllMapPoints().size() << ",";*/ - //-- + double timeMPCreation = std::chrono::duration_cast >(time_EndMPCreation - time_EndMPCulling).count(); + vdMPCreation_ms.push_back(timeMPCreation); +#endif + + bool b_doneLBA = false; int num_FixedKF_BA = 0; - // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping + int num_OptKF_BA = 0; + int num_MPs_BA = 0; + int num_edges_BA = 0; + // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping if(!CheckNewKeyFrames() && !stopRequested()) { // 地图中关键帧数目大于2个 @@ -172,21 +171,39 @@ void LocalMapping::Run() } // 条件---------1.1、跟踪成功的内点数目大于75-----1.2、并且是单目--或--2.1、跟踪成功的内点数目大于100-----2.2、并且不是单目 bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular); - // BA优化局部地图IMU - Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(), bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2()); + Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2()); + b_doneLBA = true; } else { - // Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化 - std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); - // 局部地图BA，不包括IMU数据 + // Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化 + // 局部地图BA，不包括IMU数据 // 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BA - Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA); - std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA); + b_doneLBA = true; } + + } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLBA = std::chrono::steady_clock::now(); + + if(b_doneLBA) + { + timeLBA_ms = std::chrono::duration_cast >(time_EndLBA - time_EndMPCreation).count(); + vdLBASync_ms.push_back(timeLBA_ms); + + nLBA_exec += 1; + if(mbAbortBA) + { + nLBA_abort += 1; + } + vnLBA_edges.push_back(num_edges_BA); + vnLBA_KFopt.push_back(num_OptKF_BA); + vnLBA_KFfixed.push_back(num_FixedKF_BA); + vnLBA_MPs.push_back(num_MPs_BA); } - t5 = std::chrono::steady_clock::now(); +#endif // Initialize IMU here // Step 7 当前关键帧所在地图的IMU初始化 @@ -204,8 +221,13 @@ void LocalMapping::Run() // 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到 KeyFrameCulling(); - t6 = std::chrono::steady_clock::now(); - // Step 9 如果累计时间差小于100s 并且 是IMU模式，进行VIBA +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndKFCulling = std::chrono::steady_clock::now(); + + timeKFCulling_ms = std::chrono::duration_cast >(time_EndKFCulling - time_EndLBA).count(); + vdKFCullingSync_ms.push_back(timeKFCulling_ms); +#endif + // Step 9 如果累计时间差小于100s 并且 是IMU模式，进行VIBA if ((mTinit<100.0f) && mbInertial) { // Step 9.1 根据条件判断是否进行VIBA1 @@ -262,31 +284,21 @@ void LocalMapping::Run() } } - std::chrono::steady_clock::time_point t7 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + vdLBA_ms.push_back(timeLBA_ms); + vdKFCulling_ms.push_back(timeKFCulling_ms); +#endif // Step 10 将当前帧加入到闭环检测队列中 mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); - std::chrono::steady_clock::time_point t8 = std::chrono::steady_clock::now(); - double t_procKF = std::chrono::duration_cast >(t1 - t0).count(); - double t_MPcull = std::chrono::duration_cast >(t2 - t1).count(); - double t_CheckMP = std::chrono::duration_cast >(t3 - t2).count(); - double t_searchNeigh = std::chrono::duration_cast >(t4 - t3).count(); - double t_Opt = std::chrono::duration_cast >(t5 - t4).count(); - double t_KF_cull = std::chrono::duration_cast >(t6 - t5).count(); - double t_Insert = std::chrono::duration_cast >(t8 - t7).count(); - //DEBUG-- - /*f_lm << setprecision(6); - f_lm << t_procKF << ","; - f_lm << t_MPcull << ","; - f_lm << t_CheckMP << ","; - f_lm << t_searchNeigh << ","; - f_lm << t_Opt << ","; - f_lm << t_KF_cull << ","; - f_lm << setprecision(0) << num_FixedKF_BA << "\n";*/ - //-- +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLocalMap = std::chrono::steady_clock::now(); + double timeLocalMap = std::chrono::duration_cast >(time_EndLocalMap - time_StartProcessKF).count(); + vdLMTotal_ms.push_back(timeLocalMap); +#endif } else if(Stop() && !mbBadImu) // 当要终止当前线程的时候 { @@ -493,15 +505,16 @@ void LocalMapping::CreateNewMapPoints() ORBmatcher matcher(th,false); // 取出当前帧从世界坐标系到相机坐标系的变换矩阵 - cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation(); - cv::Mat Rwc1 = Rcw1.t(); - cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation(); - cv::Mat Tcw1(3,4,CV_32F); - Rcw1.copyTo(Tcw1.colRange(0,3)); - tcw1.copyTo(Tcw1.col(3)); + auto Rcw1 = mpCurrentKeyFrame->GetRotation_(); + auto Rwc1 = Rcw1.t(); + auto tcw1 = mpCurrentKeyFrame->GetTranslation_(); + cv::Matx44f Tcw1{Rcw1(0,0),Rcw1(0,1),Rcw1(0,2),tcw1(0), + Rcw1(1,0),Rcw1(1,1),Rcw1(1,2),tcw1(1), + Rcw1(2,0),Rcw1(2,1),Rcw1(2,2),tcw1(2), + 0.f,0.f,0.f,1.f}; // 得到当前关键帧（左目）光心在世界坐标系中的坐标、内参 - cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + auto Ow1 = mpCurrentKeyFrame->GetCameraCenter_(); const float &fx1 = mpCurrentKeyFrame->fx; const float &fy1 = mpCurrentKeyFrame->fy; @@ -527,9 +540,9 @@ void LocalMapping::CreateNewMapPoints() // Check first that baseline is not too short // 邻接的关键帧光心在世界坐标系中的坐标 - cv::Mat Ow2 = pKF2->GetCameraCenter(); + auto Ow2 = pKF2->GetCameraCenter_(); // 基线向量，两个关键帧间的相机位移 - cv::Mat vBaseline = Ow2-Ow1; + auto vBaseline = Ow2-Ow1; // 基线长度 const float baseline = cv::norm(vBaseline); @@ -555,7 +568,7 @@ void LocalMapping::CreateNewMapPoints() // Compute Fundamental Matrix // Step 4：根据两个关键帧的位姿计算它们之间的基础矩阵 - cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2); + auto F12 = ComputeF12_(mpCurrentKeyFrame,pKF2); // Search matches that fullfil epipolar constraint // Step 5：通过BoW对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对 @@ -563,14 +576,16 @@ void LocalMapping::CreateNewMapPoints() bool bCoarse = mbInertial && ((!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && mpCurrentKeyFrame->GetMap()->GetIniertialBA1())|| mpTracker->mState==Tracking::RECENTLY_LOST); - matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse); - cv::Mat Rcw2 = pKF2->GetRotation(); - cv::Mat Rwc2 = Rcw2.t(); - cv::Mat tcw2 = pKF2->GetTranslation(); - cv::Mat Tcw2(3,4,CV_32F); - Rcw2.copyTo(Tcw2.colRange(0,3)); - tcw2.copyTo(Tcw2.col(3)); + matcher.SearchForTriangulation_(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse); + + auto Rcw2 = pKF2->GetRotation_(); + auto Rwc2 = Rcw2.t(); + auto tcw2 = pKF2->GetTranslation_(); + cv::Matx44f Tcw2{Rcw2(0,0),Rcw2(0,1),Rcw2(0,2),tcw2(0), + Rcw2(1,0),Rcw2(1,1),Rcw2(1,2),tcw2(1), + Rcw2(2,0),Rcw2(2,1),Rcw2(2,2),tcw2(2), + 0.f,0.f,0.f,1.f}; const float &fx2 = pKF2->fx; const float &fy2 = pKF2->fy; @@ -612,65 +627,65 @@ void LocalMapping::CreateNewMapPoints() if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){ if(bRight1 && bRight2){ - Rcw1 = mpCurrentKeyFrame->GetRightRotation(); + Rcw1 = mpCurrentKeyFrame->GetRightRotation_(); Rwc1 = Rcw1.t(); - tcw1 = mpCurrentKeyFrame->GetRightTranslation(); - Tcw1 = mpCurrentKeyFrame->GetRightPose(); - Ow1 = mpCurrentKeyFrame->GetRightCameraCenter(); + tcw1 = mpCurrentKeyFrame->GetRightTranslation_(); + Tcw1 = mpCurrentKeyFrame->GetRightPose_(); + Ow1 = mpCurrentKeyFrame->GetRightCameraCenter_(); - Rcw2 = pKF2->GetRightRotation(); + Rcw2 = pKF2->GetRightRotation_(); Rwc2 = Rcw2.t(); - tcw2 = pKF2->GetRightTranslation(); - Tcw2 = pKF2->GetRightPose(); - Ow2 = pKF2->GetRightCameraCenter(); + tcw2 = pKF2->GetRightTranslation_(); + Tcw2 = pKF2->GetRightPose_(); + Ow2 = pKF2->GetRightCameraCenter_(); pCamera1 = mpCurrentKeyFrame->mpCamera2; pCamera2 = pKF2->mpCamera2; } else if(bRight1 && !bRight2){ - Rcw1 = mpCurrentKeyFrame->GetRightRotation(); + Rcw1 = mpCurrentKeyFrame->GetRightRotation_(); Rwc1 = Rcw1.t(); - tcw1 = mpCurrentKeyFrame->GetRightTranslation(); - Tcw1 = mpCurrentKeyFrame->GetRightPose(); - Ow1 = mpCurrentKeyFrame->GetRightCameraCenter(); + tcw1 = mpCurrentKeyFrame->GetRightTranslation_(); + Tcw1 = mpCurrentKeyFrame->GetRightPose_(); + Ow1 = mpCurrentKeyFrame->GetRightCameraCenter_(); - Rcw2 = pKF2->GetRotation(); + Rcw2 = pKF2->GetRotation_(); Rwc2 = Rcw2.t(); - tcw2 = pKF2->GetTranslation(); - Tcw2 = pKF2->GetPose(); - Ow2 = pKF2->GetCameraCenter(); + tcw2 = pKF2->GetTranslation_(); + Tcw2 = pKF2->GetPose_(); + Ow2 = pKF2->GetCameraCenter_(); pCamera1 = mpCurrentKeyFrame->mpCamera2; pCamera2 = pKF2->mpCamera; } else if(!bRight1 && bRight2){ - Rcw1 = mpCurrentKeyFrame->GetRotation(); + Rcw1 = mpCurrentKeyFrame->GetRotation_(); Rwc1 = Rcw1.t(); - tcw1 = mpCurrentKeyFrame->GetTranslation(); - Tcw1 = mpCurrentKeyFrame->GetPose(); - Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + tcw1 = mpCurrentKeyFrame->GetTranslation_(); + Tcw1 = mpCurrentKeyFrame->GetPose_(); + Ow1 = mpCurrentKeyFrame->GetCameraCenter_(); - Rcw2 = pKF2->GetRightRotation(); + Rcw2 = pKF2->GetRightRotation_(); Rwc2 = Rcw2.t(); - tcw2 = pKF2->GetRightTranslation(); - Tcw2 = pKF2->GetRightPose(); - Ow2 = pKF2->GetRightCameraCenter(); + tcw2 = pKF2->GetRightTranslation_(); + Tcw2 = pKF2->GetRightPose_(); + Ow2 = pKF2->GetRightCameraCenter_(); pCamera1 = mpCurrentKeyFrame->mpCamera; pCamera2 = pKF2->mpCamera2; } else{ - Rcw1 = mpCurrentKeyFrame->GetRotation(); + Rcw1 = mpCurrentKeyFrame->GetRotation_(); Rwc1 = Rcw1.t(); - tcw1 = mpCurrentKeyFrame->GetTranslation(); - Tcw1 = mpCurrentKeyFrame->GetPose(); - Ow1 = mpCurrentKeyFrame->GetCameraCenter(); + tcw1 = mpCurrentKeyFrame->GetTranslation_(); + Tcw1 = mpCurrentKeyFrame->GetPose_(); + Ow1 = mpCurrentKeyFrame->GetCameraCenter_(); - Rcw2 = pKF2->GetRotation(); + Rcw2 = pKF2->GetRotation_(); Rwc2 = Rcw2.t(); - tcw2 = pKF2->GetTranslation(); - Tcw2 = pKF2->GetPose(); - Ow2 = pKF2->GetCameraCenter(); + tcw2 = pKF2->GetTranslation_(); + Tcw2 = pKF2->GetPose_(); + Ow2 = pKF2->GetCameraCenter_(); pCamera1 = mpCurrentKeyFrame->mpCamera; pCamera2 = pKF2->mpCamera; @@ -680,11 +695,11 @@ void LocalMapping::CreateNewMapPoints() // Check parallax between rays // Step 6.2：利用匹配点反投影得到视差角 // 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合 - cv::Mat xn1 = pCamera1->unprojectMat(kp1.pt); - cv::Mat xn2 = pCamera2->unprojectMat(kp2.pt); + auto xn1 = pCamera1->unprojectMat_(kp1.pt); + auto xn2 = pCamera2->unprojectMat_(kp2.pt); // 由相机坐标系转到世界坐标系(得到的是那条反投影射线的一个同向向量在世界坐标系下的表示,还是只能够表示方向)，得到视差角余弦值 - cv::Mat ray1 = Rwc1*xn1; - cv::Mat ray2 = Rwc2*xn2; + auto ray1 = Rwc1*xn1; + auto ray2 = Rwc2*xn2; // 这个就是求向量之间角度公式 const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2)); @@ -706,68 +721,70 @@ void LocalMapping::CreateNewMapPoints() // 得到双目观测的视差角 cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2); - - // Step 6.4：三角化恢复3D点 - cv::Mat x3D; - // cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1° - // cosParallaxRays < cosParallaxStereo 表明视差角很小 - // ?视差角度小时用三角法恢复3D点，视差角大时用双目恢复3D点（双目以及深度有效） - // 参考：https://github.com/raulmur/ORB_SLAM2/issues/345 + // Step 6.4：三角化恢复3D点 + cv::Matx31f x3D; + bool bEstimated = false; if(cosParallaxRays0 && (bStereo1 || bStereo2 || (cosParallaxRays<0.9998 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial))) { // Linear Triangulation Method - // 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵 - cv::Mat A(4,4,CV_32F); - A.row(0) = xn1.at(0)*Tcw1.row(2)-Tcw1.row(0); - A.row(1) = xn1.at(1)*Tcw1.row(2)-Tcw1.row(1); - A.row(2) = xn2.at(0)*Tcw2.row(2)-Tcw2.row(0); - A.row(3) = xn2.at(1)*Tcw2.row(2)-Tcw2.row(1); + cv::Matx14f A_r0 = xn1(0) * Tcw1.row(2) - Tcw1.row(0); + cv::Matx14f A_r1 = xn1(1) * Tcw1.row(2) - Tcw1.row(1); + cv::Matx14f A_r2 = xn2(0) * Tcw2.row(2) - Tcw2.row(0); + cv::Matx14f A_r3 = xn2(1) * Tcw2.row(2) - Tcw2.row(1); + cv::Matx44f A{A_r0(0), A_r0(1), A_r0(2), A_r0(3), + A_r1(0), A_r1(1), A_r1(2), A_r1(3), + A_r2(0), A_r2(1), A_r2(2), A_r2(3), + A_r3(0), A_r3(1), A_r3(2), A_r3(3)}; - cv::Mat w,u,vt; + cv::Matx44f u,vt; + cv::Matx41f w; cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV); - x3D = vt.row(3).t(); - // 归一化之前的检查 - if(x3D.at(3)==0) + cv::Matx41f x3D_h = vt.row(3).t(); + + if(x3D_h(3)==0) continue; // 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标 // Euclidean coordinates - x3D = x3D.rowRange(0,3)/x3D.at(3); + x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3); + bEstimated = true; } else if(bStereo1 && cosParallaxStereo1UnprojectStereo(idx1); + x3D = mpCurrentKeyFrame->UnprojectStereo_(idx1); + bEstimated = true; } else if(bStereo2 && cosParallaxStereo2UnprojectStereo(idx2); + x3D = pKF2->UnprojectStereo_(idx2); + bEstimated = true; } else { continue; //No stereo and very low parallax } // 为方便后续计算，转换成为了行向量 - cv::Mat x3Dt = x3D.t(); + cv::Matx13f x3Dt = x3D.t(); - if(x3Dt.empty()) continue; + if(!bEstimated) continue; //Check triangulation in front of cameras // Step 6.5：检测生成的3D点是否在相机前方,不在的话就放弃这个点 - float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at(2); + float z1 = Rcw1.row(2).dot(x3Dt)+tcw1(2); if(z1<=0) continue; - float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at(2); + float z2 = Rcw2.row(2).dot(x3Dt)+tcw2(2); if(z2<=0) continue; //Check reprojection error in first keyframe // Step 6.6：计算3D点在当前关键帧下的重投影误差 const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave]; - const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at(0); - const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at(1); + const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1(0); + const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1(1); const float invz1 = 1.0/z1; if(!bStereo1) @@ -799,8 +816,8 @@ void LocalMapping::CreateNewMapPoints() //Check reprojection error in second keyframe // 计算3D点在另一个关键帧下的重投影误差，操作同上 const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave]; - const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at(0); - const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at(1); + const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2(0); + const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2(1); const float invz2 = 1.0/z2; if(!bStereo2) { @@ -826,10 +843,10 @@ void LocalMapping::CreateNewMapPoints() // Step 6.7：检查尺度连续性 // 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点 - cv::Mat normal1 = x3D-Ow1; + auto normal1 = x3D-Ow1; float dist1 = cv::norm(normal1); - cv::Mat normal2 = x3D-Ow2; + auto normal2 = x3D-Ow2; float dist2 = cv::norm(normal2); if(dist1==0 || dist2==0) @@ -847,8 +864,9 @@ void LocalMapping::CreateNewMapPoints() continue; // Triangulation is succesfull - // Step 6.8：三角化生成3D点成功，构造成MapPoint - MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpAtlas->GetCurrentMap()); + // Step 6.8：三角化生成3D点成功，构造成MapPoint + cv::Mat x3D_(x3D); + MapPoint* pMP = new MapPoint(x3D_,mpCurrentKeyFrame,mpAtlas->GetCurrentMap()); // Step 6.9：为该MapPoint添加属性： // a.观测到该MapPoint的关键帧 @@ -1044,8 +1062,26 @@ cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) const cv::Mat &K1 = pKF1->mpCamera->toK(); const cv::Mat &K2 = pKF2->mpCamera->toK(); - // Essential Matrix: t12叉乘R12 - // Fundamental Matrix: inv(K1)*E*inv(K2) + + return K1.t().inv()*t12x*R12*K2.inv(); +} + +cv::Matx33f LocalMapping::ComputeF12_(KeyFrame *&pKF1, KeyFrame *&pKF2) +{ + auto R1w = pKF1->GetRotation_(); + auto t1w = pKF1->GetTranslation_(); + auto R2w = pKF2->GetRotation_(); + auto t2w = pKF2->GetTranslation_(); + + auto R12 = R1w*R2w.t(); + auto t12 = -R1w*R2w.t()*t2w+t1w; + + auto t12x = SkewSymmetricMatrix_(t12); + + const auto &K1 = pKF1->mpCamera->toK_(); + const auto &K2 = pKF2->mpCamera->toK_(); + + return K1.t().inv()*t12x*R12*K2.inv(); } @@ -1322,7 +1358,15 @@ cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v) -v.at(1), v.at(0), 0); } -// 请求当前线程复位,由外部线程调用,堵塞的 +cv::Matx33f LocalMapping::SkewSymmetricMatrix_(const cv::Matx31f &v) +{ + cv::Matx33f skew{0.f, -v(2), v(1), + v(2), 0.f, -v(0), + -v(1), v(0), 0.f}; + + return skew; +} + void LocalMapping::RequestReset() { { diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 90ccccd..280e90a 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -73,7 +73,25 @@ void LoopClosing::Run() mpLastCurrentKF->mvpLoopCandKFs.clear(); mpLastCurrentKF->mvpMergeCandKFs.clear(); } - if(NewDetectCommonRegions()) +#ifdef REGISTER_TIMES + timeDetectBoW = 0; + std::chrono::steady_clock::time_point time_StartDetectBoW = std::chrono::steady_clock::now(); +#endif + bool bDetected = NewDetectCommonRegions(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndDetectBoW = std::chrono::steady_clock::now(); + double timeDetect = std::chrono::duration_cast >(time_EndDetectBoW - time_StartDetectBoW).count(); + double timeDetectSE3 = timeDetect - timeDetectBoW; + + if(timeDetectBoW > 0) + { + vTimeBoW_ms.push_back(timeDetectBoW); + } + vTimeSE3_ms.push_back(timeDetectSE3); + vTimePRTotal_ms.push_back(timeDetect); +#endif + + if(bDetected) { if(mbMergeDetected) { @@ -119,21 +137,22 @@ void LoopClosing::Run() } } - -// cout << "tw2w1: " << mSold_new.translation() << endl; -// cout << "Rw2w1: " << mSold_new.rotation().toRotationMatrix() << endl; -// cout << "Angle Rw2w1: " << 180*LogSO3(mSold_new.rotation().toRotationMatrix())/3.14 << endl; -// cout << "scale w2w1: " << mSold_new.scale() << endl; - mg2oMergeSmw = gSmw2 * gSw2c * gScw1; mg2oMergeScw = mg2oMergeSlw; - // TODO UNCOMMENT +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now(); +#endif if (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) MergeLocal2(); else MergeLocal(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMerge = std::chrono::steady_clock::now(); + double timeMerge = std::chrono::duration_cast >(time_EndMerge - time_StartMerge).count(); + vTimeMergeTotal_ms.push_back(timeMerge); +#endif } vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp); @@ -201,7 +220,16 @@ void LoopClosing::Run() } mvpLoopMapPoints = mvpLoopMPs;//*mvvpLoopMapPoints[nCurrentIndex]; + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLoop = std::chrono::steady_clock::now(); +#endif CorrectLoop(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLoop = std::chrono::steady_clock::now(); + double timeLoop = std::chrono::duration_cast >(time_EndLoop - time_StartLoop).count(); + vTimeLoopTotal_ms.push_back(timeLoop); +#endif } else { @@ -211,7 +239,16 @@ void LoopClosing::Run() else { mvpLoopMapPoints = mvpLoopMPs; +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLoop = std::chrono::steady_clock::now(); +#endif CorrectLoop(); + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLoop = std::chrono::steady_clock::now(); + double timeLoop = std::chrono::duration_cast >(time_EndLoop - time_StartLoop).count(); + vTimeLoopTotal_ms.push_back(timeLoop); +#endif } // Reset all variables @@ -475,8 +512,15 @@ bool LoopClosing::NewDetectCommonRegions() //cout << "LC: bMergeDetectedInKF: " << bMergeDetectedInKF << " bLoopDetectedInKF: " << bLoopDetectedInKF << endl; if(!bMergeDetectedInKF || !bLoopDetectedInKF) { +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartDetectBoW = std::chrono::steady_clock::now(); +#endif // Search in BoW mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndDetectBoW = std::chrono::steady_clock::now(); + timeDetectBoW = std::chrono::duration_cast >(time_EndDetectBoW - time_StartDetectBoW).count(); +#endif } // Check the BoW candidates if the geometric candidate list is empty @@ -520,7 +564,6 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* { set spAlreadyMatchedMPs; nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs); - cout << "REFFINE-SIM3: Projection from last KF with " << nNumProjMatches << " matches" << endl; int nProjMatches = 30; @@ -546,7 +589,6 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2()) bFixedScale=false; int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true); - cout << "REFFINE-SIM3: Optimize Sim3 from last KF with " << numOptMatches << " inliers" << endl; @@ -744,7 +786,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, vector vpMatchedKF; vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5); - cout <<"BoW: " << numProjMatches << " matches between " << vpMapPoints.size() << " points with coarse Sim3" << endl; if(numProjMatches >= nProjMatches) { @@ -773,44 +814,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(numProjOptMatches >= nProjOptMatches) { - cout << "BoW: Current KF " << mpCurrentKF->mnId << "; candidate KF " << pKFi->mnId << endl; - cout << "BoW: There are " << numProjOptMatches << " matches between them with the optimized Sim3" << endl; - int max_x = -1, min_x = 1000000; - int max_y = -1, min_y = 1000000; - for(MapPoint* pMPi : vpMatchedMP) - { - if(!pMPi || pMPi->isBad()) - { - continue; - } - - tuple indexes = pMPi->GetIndexInKeyFrame(pKFi); - int index = get<0>(indexes); - if(index >= 0) - { - int coord_x = pKFi->mvKeysUn[index].pt.x; - if(coord_x < min_x) - { - min_x = coord_x; - } - if(coord_x > max_x) - { - max_x = coord_x; - } - int coord_y = pKFi->mvKeysUn[index].pt.y; - if(coord_y < min_y) - { - min_y = coord_y; - } - if(coord_y > max_y) - { - max_y = coord_y; - } - } - } - //cout << "BoW: Coord in X -> " << min_x << ", " << max_x << "; and Y -> " << min_y << ", " << max_y << endl; - //cout << "BoW: features area in X -> " << (max_x - min_x) << " and Y -> " << (max_y - min_y) << endl; - int nNumKFs = 0; //vpMatchedMPs = vpMatchedMP; //vpMPs = vpMapPoints; @@ -832,27 +835,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, if(bValid) { - //cout << "BoW: KF " << pKFj->mnId << " has " << numProjMatches_j << " matches" << endl; - cv::Mat Tc_w = mpCurrentKF->GetPose(); - cv::Mat Tw_cj = pKFj->GetPoseInverse(); - cv::Mat Tc_cj = Tc_w * Tw_cj; - cv::Vec3d vector_dist = Tc_cj.rowRange(0, 3).col(3); - cv::Mat Rc_cj = Tc_cj.rowRange(0, 3).colRange(0, 3); - double dist = cv::norm(vector_dist); - cout << "BoW: KF " << pKFi->mnId << " to KF " << pKFj->mnId << " is separated by " << dist << " meters" << endl; - cout << "BoW: Rotation between KF -> " << Rc_cj << endl; - vector v_euler = Converter::toEuler(Rc_cj); - v_euler[0] *= 180 /3.1415; - v_euler[1] *= 180 /3.1415; - v_euler[2] *= 180 /3.1415; - cout << "BoW: Rotation in angles (x, y, z) -> (" << v_euler[0] << ", " << v_euler[1] << ", " << v_euler[2] << ")" << endl; nNumKFs++; - /*if(numProjMatches_j > numProjOptMatches) - { - pLastCurrentKF = pKFj; - g2oScw = gSjw; - vpMatchedMPs = vpMatchedMPs_j; - }*/ } j++; @@ -1067,9 +1050,7 @@ void LoopClosing::CorrectLoop() // Step 1：根据共视关系更新当前关键帧与其它关键帧之间的连接关系 // 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新 cout << "start updating connections" << endl; - assert(mpCurrentKF->GetMap()->CheckEssentialGraph()); mpCurrentKF->UpdateConnections(); - assert(mpCurrentKF->GetMap()->CheckEssentialGraph()); // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation // Step 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点 @@ -1129,8 +1110,6 @@ void LoopClosing::CorrectLoop() } // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop - cout << "LC: start correcting KeyFrames" << endl; - cout << "LC: there are " << CorrectedSim3.size() << " KFs in the local window" << endl; // Step 2.2：得到矫正的当前关键帧的共视关键帧位姿后，修正这些关键帧的地图点 // 遍历待矫正的共视关键帧（不包括当前关键帧） for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++) @@ -1201,13 +1180,11 @@ void LoopClosing::CorrectLoop() pKFi->UpdateConnections(); } // TODO Check this index increasement - mpAtlas->GetCurrentMap()->IncreaseChangeIndex(); - cout << "LC: end correcting KeyFrames" << endl; + pLoopMap->IncreaseChangeIndex(); // Start Loop Fusion // Update matched map points and replace if duplicated - cout << "LC: start replacing duplicated" << endl; // Step 3：检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补 // mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点 for(size_t i=0; iSetStepByStep(true); + Verbose::PrintMess("MERGE: Merge Visual detected!!!!", Verbose::VERBOSITY_NORMAL); int numTemporalKFs = 15; //TODO (set by parameter): Temporal KFs in the local window if the map is inertial. @@ -1345,7 +1320,7 @@ void LoopClosing::MergeLocal() // Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge bool bRelaunchBA = false; - Verbose::PrintMess("MERGE-VISUAL: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG); // If a Global Bundle Adjustment is running, abort it if(isRunningGBA()) { @@ -1362,14 +1337,14 @@ void LoopClosing::MergeLocal() bRelaunchBA = true; } - Verbose::PrintMess("MERGE-VISUAL: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG); mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper->isStopped()) { usleep(1000); } - Verbose::PrintMess("MERGE-VISUAL: Local Map stopped", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Local Map stopped", Verbose::VERBOSITY_DEBUG); mpLocalMapper->EmptyQueue(); @@ -1378,12 +1353,6 @@ void LoopClosing::MergeLocal() Map* pCurrentMap = mpCurrentKF->GetMap(); Map* pMergeMap = mpMergeMatchedKF->GetMap(); - Verbose::PrintMess("MERGE-VISUAL: Initially there are " + to_string(pCurrentMap->KeyFramesInMap()) + " KFs and " + to_string(pCurrentMap->MapPointsInMap()) + " MPs in the active map ", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: Initially there are " + to_string(pMergeMap->KeyFramesInMap()) + " KFs and " + to_string(pMergeMap->MapPointsInMap()) + " MPs in the matched map ", Verbose::VERBOSITY_DEBUG); - //vector vpMergeKFs = pMergeMap->GetAllKeyFrames(); - - //// - // Ensure current keyframe is updated mpCurrentKF->UpdateConnections(); @@ -1421,7 +1390,6 @@ void LoopClosing::MergeLocal() vector vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs); spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); - Verbose::PrintMess("MERGE-VISUAL: Initial number of KFs in local window from active map: " + to_string(spLocalWindowKFs.size()), Verbose::VERBOSITY_DEBUG); const int nMaxTries = 3; int nNumTries = 0; while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries) @@ -1444,11 +1412,6 @@ void LoopClosing::MergeLocal() spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); nNumTries++; } - Verbose::PrintMess("MERGE-VISUAL: Last number of KFs in local window from the active map: " + to_string(spLocalWindowKFs.size()), Verbose::VERBOSITY_DEBUG); - - //TODO TEST - //vector vpTestKFs = pCurrentMap->GetAllKeyFrames(); - //spLocalWindowKFs.insert(vpTestKFs.begin(), vpTestKFs.end()); for(KeyFrame* pKFi : spLocalWindowKFs) { @@ -1458,10 +1421,7 @@ void LoopClosing::MergeLocal() set spMPs = pKFi->GetMapPoints(); spLocalWindowMPs.insert(spMPs.begin(), spMPs.end()); } - Verbose::PrintMess("MERGE-VISUAL: Number of MPs in local window from active map: " + to_string(spLocalWindowMPs.size()), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: Number of MPs in the active map: " + to_string(pCurrentMap->GetAllMapPoints().size()), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("-------", Verbose::VERBOSITY_DEBUG); set spMergeConnectedKFs; if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization { @@ -1486,7 +1446,6 @@ void LoopClosing::MergeLocal() } vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs); spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end()); - Verbose::PrintMess("MERGE-VISUAL: Initial number of KFs in the local window from matched map: " + to_string(spMergeConnectedKFs.size()), Verbose::VERBOSITY_DEBUG); nNumTries = 0; while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries) { @@ -1507,7 +1466,6 @@ void LoopClosing::MergeLocal() spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end()); nNumTries++; } - Verbose::PrintMess("MERGE-VISUAL: Last number of KFs in the localwindow from matched map: " + to_string(spMergeConnectedKFs.size()), Verbose::VERBOSITY_DEBUG); set spMapPointMerge; for(KeyFrame* pKFi : spMergeConnectedKFs) @@ -1533,24 +1491,13 @@ void LoopClosing::MergeLocal() vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw; vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw; - - //TODO Time test -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeStartTransfMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeStartTransfMerge = std::chrono::monotonic_clock::now(); -#endif for(KeyFrame* pKFi : spLocalWindowKFs) { if(!pKFi || pKFi->isBad()) { - Verbose::PrintMess("Bad KF in correction", Verbose::VERBOSITY_DEBUG); continue; } - if(pKFi->GetMap() != pCurrentMap) - Verbose::PrintMess("Other map KF, this should't happen", Verbose::VERBOSITY_DEBUG); - g2o::Sim3 g2oCorrectedSiw; if(pKFi!=mpCurrentKF) @@ -1583,32 +1530,16 @@ void LoopClosing::MergeLocal() pKFi->mfScale = s; eigt *=(1./s); //[R t/s;0 1] - //cout << "R: " << mg2oMergeScw.rotation().toRotationMatrix() << endl; - //cout << "angle: " << 180*LogSO3(mg2oMergeScw.rotation().toRotationMatrix())/3.14 << endl; - //cout << "t: " << mg2oMergeScw.translation() << endl; - cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt); pKFi->mTcwMerge = correctedTiw; - //pKFi->SetPose(correctedTiw); - - // Make sure connections are updated - //pKFi->UpdateMap(pMergeMap); - //pMergeMap->AddKeyFrame(pKFi); - //pCurrentMap->EraseKeyFrame(pKFi); - - //cout << "After -> Map current: " << pCurrentMap << "; New map: " << pKFi->GetMap() << endl; - if(pCurrentMap->isImuInitialized()) { Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix(); pKFi->mVwbMerge = Converter::toCvMat(Rcor)*pKFi->GetVelocity(); - //pKFi->SetVelocity(Converter::toCvMat(Rcor)*pKFi->GetVelocity()); // TODO: should add here scale s } - //TODO DEBUG to know which are the KFs that had been moved to the other map - //pKFi->mnOriginMapId = 5; } for(MapPoint* pMPi : spLocalWindowMPs) @@ -1630,30 +1561,9 @@ void LoopClosing::MergeLocal() cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw); pMPi->mPosMerge = cvCorrectedP3Dw; - //cout << "Rcor: " << Rcor << endl; - //cout << "Normal: " << pMPi->GetNormal() << endl; pMPi->mNormalVectorMerge = Converter::toCvMat(Rcor) * pMPi->GetNormal(); - //pMPi->SetWorldPos(cvCorrectedP3Dw); - //pMPi->UpdateMap(pMergeMap); - //pMergeMap->AddMapPoint(pMPi); - //pCurrentMap->EraseMapPoint(pMPi); - //pMPi->UpdateNormalAndDepth(); } -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeFinishTransfMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeFinishTransfMerge = std::chrono::monotonic_clock::now(); -#endif - std::chrono::duration timeTransfMerge = timeFinishTransfMerge - timeStartTransfMerge; // Time in milliseconds - Verbose::PrintMess("MERGE-VISUAL: TRANSF ms: " + to_string(timeTransfMerge.count()), Verbose::VERBOSITY_DEBUG); - - //TODO Time test -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeStartCritMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeStartCritMerge = std::chrono::monotonic_clock::now(); -#endif { unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map @@ -1692,25 +1602,13 @@ void LoopClosing::MergeLocal() pMPi->UpdateMap(pMergeMap); pMergeMap->AddMapPoint(pMPi); pCurrentMap->EraseMapPoint(pMPi); - //pMPi->UpdateNormalAndDepth(); } mpAtlas->ChangeMap(pMergeMap); mpAtlas->SetMapBad(pCurrentMap); pMergeMap->IncreaseChangeIndex(); - //TODO for debug - pMergeMap->ChangeId(pCurrentMap->GetId()); } -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeFinishCritMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeFinishCritMerge = std::chrono::monotonic_clock::now(); -#endif - std::chrono::duration timeCritMerge = timeFinishCritMerge - timeStartCritMerge; // Time in milliseconds - Verbose::PrintMess("MERGE-VISUAL: New current map: " + to_string(pMergeMap->GetId()), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: CRITICAL ms: " + to_string(timeCritMerge.count()), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: LOCAL MAPPING number of KFs: " + to_string(mpLocalMapper->KeyframesInQueue()), Verbose::VERBOSITY_DEBUG); //Rebuild the essential graph in the local window pCurrentMap->GetOriginKF()->SetFirstConnection(false); @@ -1741,29 +1639,12 @@ void LoopClosing::MergeLocal() vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); - - //TODO Time test -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeStartFuseMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeStartFuseMerge = std::chrono::monotonic_clock::now(); -#endif - // Project MapPoints observed in the neighborhood of the merge keyframe // into the current keyframe and neighbors using corrected poses. // Fuse duplications. SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint); -#ifdef COMPILEDWITHC11 - std::chrono::steady_clock::time_point timeFinishFuseMerge = std::chrono::steady_clock::now(); -#else - std::chrono::monotonic_clock::time_point timeFinishFuseMerge = std::chrono::monotonic_clock::now(); -#endif - std::chrono::duration timeFuseMerge = timeFinishFuseMerge - timeStartFuseMerge; // Time in milliseconds - Verbose::PrintMess("MERGE-VISUAL: FUSE DUPLICATED ms: " + to_string(timeFuseMerge.count()), Verbose::VERBOSITY_DEBUG); - // Update connectivity - Verbose::PrintMess("MERGE-VISUAL: Init to update connections in the welding area", Verbose::VERBOSITY_DEBUG); for(KeyFrame* pKFi : spLocalWindowKFs) { if(!pKFi || pKFi->isBad()) @@ -1779,34 +1660,25 @@ void LoopClosing::MergeLocal() pKFi->UpdateConnections(); } - //CheckObservations(spLocalWindowKFs, spMergeConnectedKFs); - - Verbose::PrintMess("MERGE-VISUAL: Finish to update connections in the welding area", Verbose::VERBOSITY_DEBUG); - bool bStop = false; - Verbose::PrintMess("MERGE-VISUAL: Start local BA ", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Start local BA ", Verbose::VERBOSITY_DEBUG); vpLocalCurrentWindowKFs.clear(); vpMergeConnectedKFs.clear(); std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs)); std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs)); if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO) { - Verbose::PrintMess("MERGE-VISUAL: Visual-Inertial", Verbose::VERBOSITY_DEBUG); Optimizer::MergeInertialBA(mpLocalMapper->GetCurrKF(),mpMergeMatchedKF,&bStop, mpCurrentKF->GetMap(),vCorrectedSim3); } else { - Verbose::PrintMess("MERGE-VISUAL: Visual", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: Local current window->" + to_string(vpLocalCurrentWindowKFs.size()) + "; Local merge window->" + to_string(vpMergeConnectedKFs.size()), Verbose::VERBOSITY_DEBUG); Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop); } // Loop closed. Release Local Mapping. mpLocalMapper->Release(); - - //return; - Verbose::PrintMess("MERGE-VISUAL: Finish the LBA", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Finish the LBA", Verbose::VERBOSITY_DEBUG); //// @@ -1816,11 +1688,11 @@ void LoopClosing::MergeLocal() if(vpCurrentMapKFs.size() == 0) { - Verbose::PrintMess("MERGE-VISUAL: There are not KFs outside of the welding area", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: There are not KFs outside of the welding area", Verbose::VERBOSITY_DEBUG); } else { - Verbose::PrintMess("MERGE-VISUAL: Calculate the new position of the elements outside of the window", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE: Calculate the new position of the elements outside of the window", Verbose::VERBOSITY_DEBUG); //Apply the transformation { if(mpTracker->mSensor == System::MONOCULAR) @@ -1893,7 +1765,6 @@ void LoopClosing::MergeLocal() } } } - Verbose::PrintMess("MERGE-VISUAL: Apply transformation to all elements of the old map", Verbose::VERBOSITY_DEBUG); mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped @@ -1901,7 +1772,6 @@ void LoopClosing::MergeLocal() { usleep(1000); } - Verbose::PrintMess("MERGE-VISUAL: Local Map stopped", Verbose::VERBOSITY_DEBUG); // Optimize graph (and update the loop position for each element form the begining to the end) if(mpTracker->mSensor != System::MONOCULAR) @@ -1915,9 +1785,6 @@ void LoopClosing::MergeLocal() unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information unique_lock mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map - Verbose::PrintMess("MERGE-VISUAL: There are " + to_string(pMergeMap->KeyFramesInMap()) + " KFs in the map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: It will be inserted " + to_string(vpCurrentMapKFs.size()) + " KFs in the map", Verbose::VERBOSITY_DEBUG); - for(KeyFrame* pKFi : vpCurrentMapKFs) { if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap) @@ -1930,8 +1797,6 @@ void LoopClosing::MergeLocal() pMergeMap->AddKeyFrame(pKFi); pCurrentMap->EraseKeyFrame(pKFi); } - Verbose::PrintMess("MERGE-VISUAL: There are " + to_string(pMergeMap->MapPointsInMap()) + " MPs in the map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL: It will be inserted " + to_string(vpCurrentMapMPs.size()) + " MPs in the map", Verbose::VERBOSITY_DEBUG); for(MapPoint* pMPi : vpCurrentMapMPs) { @@ -1942,19 +1807,12 @@ void LoopClosing::MergeLocal() pMergeMap->AddMapPoint(pMPi); pCurrentMap->EraseMapPoint(pMPi); } - Verbose::PrintMess("MERGE-VISUAL: There are " + to_string(pMergeMap->MapPointsInMap()) + " MPs in the map", Verbose::VERBOSITY_DEBUG); } - - Verbose::PrintMess("MERGE-VISUAL: Optimaze the essential graph", Verbose::VERBOSITY_DEBUG); } - - mpLocalMapper->Release(); - - Verbose::PrintMess("MERGE-VISUAL: Finally there are " + to_string(pMergeMap->KeyFramesInMap()) + "KFs and " + to_string(pMergeMap->MapPointsInMap()) + " MPs in the complete map ", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("MERGE-VISUAL:Completed!!!!!", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("MERGE:Completed!!!!!", Verbose::VERBOSITY_DEBUG); if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1))) { @@ -1976,52 +1834,6 @@ void LoopClosing::MergeLocal() } -void LoopClosing::printReprojectionError(set &spLocalWindowKFs, KeyFrame* mpCurrentKF, string &name) -{ - string path_imgs = "./test_Reproj/"; - for(KeyFrame* pKFi : spLocalWindowKFs) - { - //cout << "KF " << pKFi->mnId << endl; - cv::Mat img_i = cv::imread(pKFi->mNameFile, CV_LOAD_IMAGE_UNCHANGED); - //cout << "Image -> " << img_i.cols << ", " << img_i.rows << endl; - cv::cvtColor(img_i, img_i, CV_GRAY2BGR); - //cout << "Change of color in the image " << endl; - - vector vpMPs = pKFi->GetMapPointMatches(); - int num_points = 0; - for(int j=0; jisBad()) - { - continue; - } - - cv::KeyPoint point_img = pKFi->mvKeysUn[j]; - cv::Point2f reproj_p; - float u, v; - bool bIsInImage = pKFi->ProjectPointUnDistort(pMPij, reproj_p, u, v); - if(bIsInImage){ - //cout << "Reproj in the image" << endl; - cv::circle(img_i, point_img.pt, 1/*point_img.octave*/, cv::Scalar(0, 255, 0)); - cv::line(img_i, point_img.pt, reproj_p, cv::Scalar(0, 0, 255)); - num_points++; - } - else - { - //cout << "Reproj out of the image" << endl; - cv::circle(img_i, point_img.pt, point_img.octave, cv::Scalar(0, 0, 255)); - } - - } - //cout << "Image painted" << endl; - string filename_img = path_imgs + "KF" + to_string(mpCurrentKF->mnId) + "_" + to_string(pKFi->mnId) + name + "points" + to_string(num_points) + ".png"; - cv::imwrite(filename_img, img_i); - } - -} - - void LoopClosing::MergeLocal2() { cout << "Merge detected!!!!" << endl; @@ -2078,14 +1890,9 @@ void LoopClosing::MergeLocal2() unique_lock lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); - cout << "KFs before empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl; mpLocalMapper->EmptyQueue(); - cout << "KFs after empty: " << mpAtlas->GetCurrentMap()->KeyFramesInMap() << endl; std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); - cout << "updating active map to merge reference" << endl; - cout << "curr merge KF id: " << mpCurrentKF->mnId << endl; - cout << "curr tracking KF id: " << mpTracker->GetLastKeyFrame()->mnId << endl; bool bScaleVel=false; if(s_on!=1) bScaleVel=true; @@ -2114,11 +1921,7 @@ void LoopClosing::MergeLocal2() } - - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - // Load KFs and MPs from merge map - cout << "updating current map" << endl; { // Get Merge Map Mutex (This section stops tracking!!) unique_lock currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information @@ -2164,17 +1967,6 @@ void LoopClosing::MergeLocal2() } } - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - - cout << "end updating current map" << endl; - - // Critical zone - bool good = pCurrentMap->CheckEssentialGraph(); - /*if(!good) - cout << "BAD ESSENTIAL GRAPH!!" << endl;*/ - - cout << "Update essential graph" << endl; - // mpCurrentKF->UpdateConnections(); // to put at false mbFirstConnection pMergeMap->GetOriginKF()->SetFirstConnection(false); pNewChild = mpMergeMatchedKF->GetParent(); // Old parent, it will be the new child of this KF pNewParent = mpMergeMatchedKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent) @@ -2189,33 +1981,17 @@ void LoopClosing::MergeLocal2() } - - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - - cout << "end update essential graph" << endl; - - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 1!!" << endl; - - cout << "Update relationship between KFs" << endl; vector vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge) vector vpCurrentConnectedKFs; - - mvpMergeConnectedKFs.push_back(mpMergeMatchedKF); vector aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); mvpMergeConnectedKFs.insert(mvpMergeConnectedKFs.end(), aux.begin(), aux.end()); if (mvpMergeConnectedKFs.size()>6) mvpMergeConnectedKFs.erase(mvpMergeConnectedKFs.begin()+6,mvpMergeConnectedKFs.end()); - /*mvpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); - mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);*/ mpCurrentKF->UpdateConnections(); vpCurrentConnectedKFs.push_back(mpCurrentKF); - /*vpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames(); - vpCurrentConnectedKFs.push_back(mpCurrentKF);*/ aux = mpCurrentKF->GetVectorCovisibleKeyFrames(); vpCurrentConnectedKFs.insert(vpCurrentConnectedKFs.end(), aux.begin(), aux.end()); if (vpCurrentConnectedKFs.size()>6) @@ -2230,33 +2006,10 @@ void LoopClosing::MergeLocal2() break; } - cout << "vpCurrentConnectedKFs.size() " << vpCurrentConnectedKFs.size() << endl; - cout << "mvpMergeConnectedKFs.size() " << mvpMergeConnectedKFs.size() << endl; - cout << "spMapPointMerge.size() " << spMapPointMerge.size() << endl; - - vpCheckFuseMapPoint.reserve(spMapPointMerge.size()); std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint)); - cout << "Finished to update relationship between KFs" << endl; - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 2!!" << endl; - - cout << "start SearchAndFuse" << endl; SearchAndFuse(vpCurrentConnectedKFs, vpCheckFuseMapPoint); - cout << "end SearchAndFuse" << endl; - - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 3!!" << endl; - - cout << "Init to update connections" << endl; - for(KeyFrame* pKFi : vpCurrentConnectedKFs) { @@ -2272,34 +2025,16 @@ void LoopClosing::MergeLocal2() pKFi->UpdateConnections(); } - cout << "end update connections" << endl; - cout << "MergeMap init ID: " << pMergeMap->GetInitKFid() << " CurrMap init ID: " << pCurrentMap->GetInitKFid() << endl; - - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 4!!" << endl; - - // TODO Check: If new map is too small, we suppose that not informaiton can be propagated from new to old map if (numKFnew<10){ mpLocalMapper->Release(); return; } - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 5!!" << endl; - // Perform BA bool bStopFlag=false; KeyFrame* pCurrKF = mpTracker->GetLastKeyFrame(); - cout << "start MergeInertialBA" << endl; Optimizer::MergeInertialBA(pCurrKF, mpMergeMatchedKF, &bStopFlag, pCurrentMap,CorrectedSim3); - cout << "end MergeInertialBA" << endl; - - good = pCurrentMap->CheckEssentialGraph(); - if(!good) - cout << "BAD ESSENTIAL GRAPH 6!!" << endl; // Release Local Mapping. mpLocalMapper->Release(); @@ -2308,64 +2043,12 @@ void LoopClosing::MergeLocal2() return; } -void LoopClosing::CheckObservations(set &spKFsMap1, set &spKFsMap2) -{ - cout << "----------------------" << endl; - for(KeyFrame* pKFi1 : spKFsMap1) - { - map mMatchedMP; - set spMPs = pKFi1->GetMapPoints(); - - for(MapPoint* pMPij : spMPs) - { - if(!pMPij || pMPij->isBad()) - { - continue; - } - - map> mMPijObs = pMPij->GetObservations(); - for(KeyFrame* pKFi2 : spKFsMap2) - { - if(mMPijObs.find(pKFi2) != mMPijObs.end()) - { - if(mMatchedMP.find(pKFi2) != mMatchedMP.end()) - { - mMatchedMP[pKFi2] = mMatchedMP[pKFi2] + 1; - } - else - { - mMatchedMP[pKFi2] = 1; - } - } - } - - } - - if(mMatchedMP.size() == 0) - { - cout << "CHECK-OBS: KF " << pKFi1->mnId << " has not any matched MP with the other map" << endl; - } - else - { - cout << "CHECK-OBS: KF " << pKFi1->mnId << " has matched MP with " << mMatchedMP.size() << " KF from the other map" << endl; - for(pair matchedKF : mMatchedMP) - { - cout << " -KF: " << matchedKF.first->mnId << ", Number of matches: " << matchedKF.second << endl; - } - } - } - cout << "----------------------" << endl; -} - - void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector &vpMapPoints) { ORBmatcher matcher(0.8); int total_replaces = 0; - cout << "FUSE: Initially there are " << vpMapPoints.size() << " MPs" << endl; - cout << "FUSE: Intially there are " << CorrectedPosesMap.size() << " KFs" << endl; for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++) { int num_replaces = 0; @@ -2396,7 +2079,6 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector total_replaces += num_replaces; } - cout << "FUSE: " << total_replaces << " MPs had been fused" << endl; } @@ -2406,8 +2088,6 @@ void LoopClosing::SearchAndFuse(const vector &vConectedKFs, vector &vConectedKFs, vectorReplace(vpMapPoints[i]); } } - cout << "FUSE-POSE: KF " << pKF->mnId << " ->" << num_replaces << " MPs fused" << endl; total_replaces += num_replaces; } - cout << "FUSE-POSE: " << total_replaces << " MPs had been fused" << endl; } // 由外部线程调用,请求复位当前线程 @@ -2513,11 +2191,22 @@ void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoop const bool bImuInit = pActiveMap->isImuInitialized(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartFGBA = std::chrono::steady_clock::now(); +#endif + if(!bImuInit) Optimizer::GlobalBundleAdjustemnt(pActiveMap,10,&mbStopGBA,nLoopKF,false); else Optimizer::FullInertialBA(pActiveMap,7,false,nLoopKF,&mbStopGBA); - // 记录GBA已经迭代次数,用来检查全局BA过程是否是因为意外结束的 + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartMapUpdate = std::chrono::steady_clock::now(); + + double timeFullGBA = std::chrono::duration_cast >(time_StartMapUpdate - time_StartFGBA).count(); + vTimeFullGBA_ms.push_back(timeFullGBA); +#endif + int idx = mnFullBAIdx; // Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false); @@ -2733,6 +2422,16 @@ void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoop mbFinishedGBA = true; mbRunningGBA = false; } + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndMapUpdate = std::chrono::steady_clock::now(); + + double timeMapUpdate = std::chrono::duration_cast >(time_EndMapUpdate - time_StartMapUpdate).count(); + vTimeMapUpdate_ms.push_back(timeMapUpdate); + + double timeGBA = std::chrono::duration_cast >(time_EndMapUpdate - time_StartFGBA).count(); + vTimeGBATotal_ms.push_back(timeGBA); +#endif } // 由外部线程调用,请求终止当前线程 diff --git a/src/Map.cc b/src/Map.cc index 7d3738b..d3193b4 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -502,154 +502,5 @@ void Map::SetLastMapChange(int currentChangeId) mnMapChangeNotified = currentChangeId; } -void Map::printReprojectionError(list &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder) -{ - string path_imgs = "./" + name_folder + "/"; - for(KeyFrame* pKFi : lpLocalWindowKFs) - { - //cout << "KF " << pKFi->mnId << endl; - cv::Mat img_i = cv::imread(pKFi->mNameFile, CV_LOAD_IMAGE_UNCHANGED); - //cout << "Image -> " << img_i.cols << ", " << img_i.rows << endl; - cv::cvtColor(img_i, img_i, CV_GRAY2BGR); - //cout << "Change of color in the image " << endl; - - vector vpMPs = pKFi->GetMapPointMatches(); - int num_points = 0; - for(int j=0; jisBad()) - { - continue; - } - - cv::KeyPoint point_img = pKFi->mvKeysUn[j]; - cv::Point2f reproj_p; - float u, v; - bool bIsInImage = pKFi->ProjectPointUnDistort(pMPij, reproj_p, u, v); - if(bIsInImage){ - //cout << "Reproj in the image" << endl; - cv::circle(img_i, point_img.pt, 1/*point_img.octave*/, cv::Scalar(0, 255, 0)); - cv::line(img_i, point_img.pt, reproj_p, cv::Scalar(0, 0, 255)); - num_points++; - } - else - { - //cout << "Reproj out of the image" << endl; - cv::circle(img_i, point_img.pt, point_img.octave, cv::Scalar(0, 0, 255)); - } - - } - //cout << "Image painted" << endl; - string filename_img = path_imgs + "KF" + to_string(mpCurrentKF->mnId) + "_" + to_string(pKFi->mnId) + name + "points" + to_string(num_points) + ".png"; - cv::imwrite(filename_img, img_i); - } - -} - -void Map::PreSave(std::set &spCams) -{ - int nMPWithoutObs = 0; - for(MapPoint* pMPi : mspMapPoints) - { - if(pMPi->GetObservations().size() == 0) - { - nMPWithoutObs++; - } - map> mpObs = pMPi->GetObservations(); - for(map>::iterator it= mpObs.begin(), end=mpObs.end(); it!=end; ++it) - { - if(it->first->GetMap() != this) - { - pMPi->EraseObservation(it->first); //We need to find where the KF is set as Bad but the observation is not removed - } - - } - } - cout << " Bad MapPoints removed" << endl; - - // Saves the id of KF origins - mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size()); - for(int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i) - { - mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId); - } - - mvpBackupMapPoints.clear(); - // Backup of set container to vector - //std::copy(mspMapPoints.begin(), mspMapPoints.end(), std::back_inserter(mvpBackupMapPoints)); - for(MapPoint* pMPi : mspMapPoints) - { - //cout << "Pre-save of mappoint " << pMPi->mnId << endl; - mvpBackupMapPoints.push_back(pMPi); - pMPi->PreSave(mspKeyFrames,mspMapPoints); - } - cout << " MapPoints back up done!!" << endl; - - mvpBackupKeyFrames.clear(); - //std::copy(mspKeyFrames.begin(), mspKeyFrames.end(), std::back_inserter(mvpBackupKeyFrames)); - for(KeyFrame* pKFi : mspKeyFrames) - { - mvpBackupKeyFrames.push_back(pKFi); - pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams); - } - cout << " KeyFrames back up done!!" << endl; - - mnBackupKFinitialID = -1; - if(mpKFinitial) - { - mnBackupKFinitialID = mpKFinitial->mnId; - } - - mnBackupKFlowerID = -1; - if(mpKFlowerID) - { - mnBackupKFlowerID = mpKFlowerID->mnId; - } - -} - -void Map::PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map& mpKeyFrameId, map &mpCams) -{ - std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin())); - std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin())); - - map mpMapPointId; - for(MapPoint* pMPi : mspMapPoints) - { - pMPi->UpdateMap(this); - mpMapPointId[pMPi->mnId] = pMPi; - } - - //map mpKeyFrameId; - for(KeyFrame* pKFi : mspKeyFrames) - { - pKFi->UpdateMap(this); - pKFi->SetORBVocabulary(pORBVoc); - pKFi->SetKeyFrameDatabase(pKFDB); - mpKeyFrameId[pKFi->mnId] = pKFi; - } - cout << "Number of KF: " << mspKeyFrames.size() << endl; - cout << "Number of MP: " << mspMapPoints.size() << endl; - - // References reconstruction between different instances - for(MapPoint* pMPi : mspMapPoints) - { - //cout << "Post-Load of mappoint " << pMPi->mnId << endl; - pMPi->PostLoad(mpKeyFrameId, mpMapPointId); - } - cout << "End to rebuild MapPoint references" << endl; - - for(KeyFrame* pKFi : mspKeyFrames) - { - pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams); - pKFDB->add(pKFi); - } - - cout << "End to rebuild KeyFrame references" << endl; - - mvpBackupMapPoints.clear(); -} - } //namespace ORB_SLAM3 diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc index 9383bb3..91c39b2 100644 --- a/src/MapDrawer.cc +++ b/src/MapDrawer.cc @@ -186,14 +186,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b glLineWidth(mKeyFrameLineWidth*5); glColor3f(1.0f,0.0f,0.0f); glBegin(GL_LINES); - - //cout << "Initial KF: " << mpAtlas->GetCurrentMap()->GetOriginKF()->mnId << endl; - //cout << "Parent KF: " << vpKFs[i]->mnId << endl; } else { glLineWidth(mKeyFrameLineWidth); - //glColor3f(0.0f,0.0f,1.0f); glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]); glBegin(GL_LINES); } @@ -222,32 +218,6 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b glPopMatrix(); - //Draw lines with Loop and Merge candidates - /*glLineWidth(mGraphLineWidth); - glColor4f(1.0f,0.6f,0.0f,1.0f); - glBegin(GL_LINES); - cv::Mat Ow = pKF->GetCameraCenter(); - const vector vpLoopCandKFs = pKF->mvpLoopCandKFs; - if(!vpLoopCandKFs.empty()) - { - for(vector::const_iterator vit=vpLoopCandKFs.begin(), vend=vpLoopCandKFs.end(); vit!=vend; vit++) - { - cv::Mat Ow2 = (*vit)->GetCameraCenter(); - glVertex3f(Ow.at(0),Ow.at(1),Ow.at(2)); - glVertex3f(Ow2.at(0),Ow2.at(1),Ow2.at(2)); - } - } - const vector vpMergeCandKFs = pKF->mvpMergeCandKFs; - if(!vpMergeCandKFs.empty()) - { - for(vector::const_iterator vit=vpMergeCandKFs.begin(), vend=vpMergeCandKFs.end(); vit!=vend; vit++) - { - cv::Mat Ow2 = (*vit)->GetCameraCenter(); - glVertex3f(Ow.at(0),Ow.at(1),Ow.at(2)); - glVertex3f(Ow2.at(0),Ow2.at(1),Ow2.at(2)); - } - }*/ - glEnd(); } } @@ -258,7 +228,6 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b glColor4f(0.0f,1.0f,0.0f,0.6f); glBegin(GL_LINES); - // cout << "-----------------Draw graph-----------------" << endl; for(size_t i=0; iGetId()) { Pos.copyTo(mWorldPos); + mWorldPosx = cv::Matx31f(Pos.at(0), Pos.at(1), Pos.at(2)); mNormalVector = cv::Mat::zeros(3,1,CV_32F); + mNormalVectorx = cv::Matx31f::zeros(); mbTrackInViewR = false; mbTrackInView = false; @@ -67,6 +69,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, mpHostKF = pHostKF; mNormalVector = cv::Mat::zeros(3,1,CV_32F); + mNormalVectorx = cv::Matx31f::zeros(); // Worldpos is not set // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id. @@ -81,6 +84,8 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap), mnOriginMapId(pMap->GetId()) { Pos.copyTo(mWorldPos); + mWorldPosx = cv::Matx31f(Pos.at(0), Pos.at(1), Pos.at(2)); + cv::Mat Ow; if(pFrame -> Nleft == -1 || idxF < pFrame -> Nleft){ Ow = pFrame->GetCameraCenter(); @@ -94,6 +99,8 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF } mNormalVector = mWorldPos - Ow; mNormalVector = mNormalVector/cv::norm(mNormalVector); + mNormalVectorx = cv::Matx31f(mNormalVector.at(0), mNormalVector.at(1), mNormalVector.at(2)); + cv::Mat PC = Pos - Ow; const float dist = cv::norm(PC); @@ -118,6 +125,7 @@ void MapPoint::SetWorldPos(const cv::Mat &Pos) unique_lock lock2(mGlobalMutex); unique_lock lock(mMutexPos); Pos.copyTo(mWorldPos); + mWorldPosx = cv::Matx31f(Pos.at(0), Pos.at(1), Pos.at(2)); } cv::Mat MapPoint::GetWorldPos() @@ -132,6 +140,18 @@ cv::Mat MapPoint::GetNormal() return mNormalVector.clone(); } +cv::Matx31f MapPoint::GetWorldPos2() +{ + unique_lock lock(mMutexPos); + return mWorldPosx; +} + +cv::Matx31f MapPoint::GetNormal2() +{ + unique_lock lock(mMutexPos); + return mNormalVectorx; +} + KeyFrame* MapPoint::GetReferenceKeyFrame() { unique_lock lock(mMutexFeatures); @@ -184,7 +204,6 @@ void MapPoint::EraseObservation(KeyFrame* pKF) // 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数 if(mObservations.count(pKF)) { - //int idx = mObservations[pKF]; tuple indexes = mObservations[pKF]; int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); @@ -566,6 +585,7 @@ void MapPoint::UpdateNormalAndDepth() mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限 mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限 mNormalVector = normal/n; // 获得地图点平均的观测方向 + mNormalVectorx = cv::Matx31f(mNormalVector.at(0), mNormalVector.at(1), mNormalVector.at(2)); } } @@ -573,6 +593,7 @@ void MapPoint::SetNormalVector(cv::Mat& normal) { unique_lock lock3(mMutexPos); mNormalVector = normal; + mNormalVectorx = cv::Matx31f(mNormalVector.at(0), mNormalVector.at(1), mNormalVector.at(2)); } float MapPoint::GetMinDistanceInvariance() @@ -646,18 +667,6 @@ int MapPoint::PredictScale(const float ¤tDist, Frame* pF) return nScale; } -void MapPoint::PrintObservations() -{ - cout << "MP_OBS: MP " << mnId << endl; - for(map>::iterator mit=mObservations.begin(), mend=mObservations.end(); mit!=mend; mit++) - { - KeyFrame* pKFi = mit->first; - tuple indexes = mit->second; - int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); - cout << "--OBS in KF " << pKFi->mnId << " in map " << pKFi->GetMap()->GetId() << endl; - } -} - Map* MapPoint::GetMap() { unique_lock lock(mMutexMap); @@ -670,66 +679,4 @@ void MapPoint::UpdateMap(Map* pMap) mpMap = pMap; } -void MapPoint::PreSave(set& spKF,set& spMP) -{ - mBackupReplacedId = -1; - if(mpReplaced && spMP.find(mpReplaced) != spMP.end()) - mBackupReplacedId = mpReplaced->mnId; - - mBackupObservationsId1.clear(); - mBackupObservationsId2.clear(); - // Save the id and position in each KF who view it - for(std::map >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it) - { - KeyFrame* pKFi = it->first; - if(spKF.find(pKFi) != spKF.end()) - { - mBackupObservationsId1[it->first->mnId] = get<0>(it->second); - mBackupObservationsId2[it->first->mnId] = get<1>(it->second); - } - else - { - EraseObservation(pKFi); - } - } - - // Save the id of the reference KF - if(spKF.find(mpRefKF) != spKF.end()) - { - mBackupRefKFId = mpRefKF->mnId; - } -} - -void MapPoint::PostLoad(map& mpKFid, map& mpMPid) -{ - mpRefKF = mpKFid[mBackupRefKFId]; - if(!mpRefKF) - { - cout << "MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl; - } - mpReplaced = static_cast(NULL); - if(mBackupReplacedId>=0) - { - map::iterator it = mpMPid.find(mBackupReplacedId); - if (it != mpMPid.end()) - mpReplaced = it->second; - } - - mObservations.clear(); - - for(map::const_iterator it = mBackupObservationsId1.begin(), end = mBackupObservationsId1.end(); it != end; ++it) - { - KeyFrame* pKFi = mpKFid[it->first]; - map::const_iterator it2 = mBackupObservationsId2.find(it->first); - std::tuple indexes = tuple(it->second,it2->second); - if(pKFi) - { - mObservations[pKFi] = indexes; - } - } - - mBackupObservationsId1.clear(); - mBackupObservationsId2.clear(); -} - } //namespace ORB_SLAM diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc index 62f939d..a071631 100644 --- a/src/ORBextractor.cc +++ b/src/ORBextractor.cc @@ -1043,7 +1043,7 @@ void ORBextractor::ComputeKeyPointsOctTree( allKeypoints.resize(nlevels); //图像cell的尺寸，是个正方形，可以理解为边长in像素坐标 - const float W = 30; + const float W = 35; // 对每一层图像做处理 //遍历所有图像 diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index b967094..86f2053 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -1385,6 +1385,250 @@ int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F return nmatches; } + int ORBmatcher::SearchForTriangulation_(KeyFrame *pKF1, KeyFrame *pKF2, cv::Matx33f F12, + vector > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse) + { + const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec; + const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec; + + //Compute epipole in second image + auto Cw = pKF1->GetCameraCenter_(); + auto R2w = pKF2->GetRotation_(); + auto t2w = pKF2->GetTranslation_(); + auto C2 = R2w*Cw+t2w; + + cv::Point2f ep = pKF2->mpCamera->project(C2); + + auto R1w = pKF1->GetRotation_(); + auto t1w = pKF1->GetTranslation_(); + + cv::Matx33f R12; + cv::Matx31f t12; + + cv::Matx33f Rll,Rlr,Rrl,Rrr; + cv::Matx31f tll,tlr,trl,trr; + + GeometricCamera* pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera; + + if(!pKF1->mpCamera2 && !pKF2->mpCamera2){ + R12 = R1w*R2w.t(); + t12 = -R1w*R2w.t()*t2w+t1w; + } + else{ + Rll = pKF1->GetRotation_() * pKF2->GetRotation_().t(); + Rlr = pKF1->GetRotation_() * pKF2->GetRightRotation_().t(); + Rrl = pKF1->GetRightRotation_() * pKF2->GetRotation_().t(); + Rrr = pKF1->GetRightRotation_() * pKF2->GetRightRotation_().t(); + + tll = pKF1->GetRotation_() * (-pKF2->GetRotation_().t() * pKF2->GetTranslation_()) + pKF1->GetTranslation_(); + tlr = pKF1->GetRotation_() * (-pKF2->GetRightRotation_().t() * pKF2->GetRightTranslation_()) + pKF1->GetTranslation_(); + trl = pKF1->GetRightRotation_() * (-pKF2->GetRotation_().t() * pKF2->GetTranslation_()) + pKF1->GetRightTranslation_(); + trr = pKF1->GetRightRotation_() * (-pKF2->GetRightRotation_().t() * pKF2->GetRightTranslation_()) + pKF1->GetRightTranslation_(); + } + + // Find matches between not tracked keypoints + // Matching speed-up by ORB Vocabulary + // Compare only ORB that share the same node + + int nmatches=0; + vector vbMatched2(pKF2->N,false); + vector vMatches12(pKF1->N,-1); + + vector rotHist[HISTO_LENGTH]; + for(int i=0;ifirst == f2it->first) + { + for(size_t i1=0, iend1=f1it->second.size(); i1second[i1]; + + MapPoint* pMP1 = pKF1->GetMapPoint(idx1); + + // If there is already a MapPoint skip + if(pMP1) + { + continue; + } + + const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0); + + if(bOnlyStereo) + if(!bStereo1) + continue; + + + const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1] + : (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1] + : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft]; + + const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false + : true; + //if(bRight1) continue; + const cv::Mat &d1 = pKF1->mDescriptors.row(idx1); + + int bestDist = TH_LOW; + int bestIdx2 = -1; + + for(size_t i2=0, iend2=f2it->second.size(); i2second[i2]; + + MapPoint* pMP2 = pKF2->GetMapPoint(idx2); + + // If we have already matched or there is a MapPoint skip + if(vbMatched2[idx2] || pMP2) + continue; + + const bool bStereo2 = (!pKF2->mpCamera2 && pKF2->mvuRight[idx2]>=0); + + if(bOnlyStereo) + if(!bStereo2) + continue; + + const cv::Mat &d2 = pKF2->mDescriptors.row(idx2); + + const int dist = DescriptorDistance(d1,d2); + + if(dist>TH_LOW || dist>bestDist) + continue; + + const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2] + : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2] + : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft]; + const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false + : true; + + if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2) + { + const float distex = ep.x-kp2.pt.x; + const float distey = ep.y-kp2.pt.y; + if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave]) + { + continue; + } + } + + if(pKF1->mpCamera2 && pKF2->mpCamera2){ + if(bRight1 && bRight2){ + R12 = Rrr; + t12 = trr; + + pCamera1 = pKF1->mpCamera2; + pCamera2 = pKF2->mpCamera2; + } + else if(bRight1 && !bRight2){ + R12 = Rrl; + t12 = trl; + + pCamera1 = pKF1->mpCamera2; + pCamera2 = pKF2->mpCamera; + } + else if(!bRight1 && bRight2){ + R12 = Rlr; + t12 = tlr; + + pCamera1 = pKF1->mpCamera; + pCamera2 = pKF2->mpCamera2; + } + else{ + R12 = Rll; + t12 = tll; + + pCamera1 = pKF1->mpCamera; + pCamera2 = pKF2->mpCamera; + } + + } + + + if(pCamera1->epipolarConstrain_(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])||bCoarse) // MODIFICATION_2 + { + bestIdx2 = idx2; + bestDist = dist; + } + } + + if(bestIdx2>=0) + { + const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[bestIdx2] + : (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2] + : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft]; + vMatches12[idx1]=bestIdx2; + nmatches++; + + if(mbCheckOrientation) + { + float rot = kp1.angle-kp2.angle; + if(rot<0.0) + rot+=360.0f; + int bin = round(rot*factor); + if(bin==HISTO_LENGTH) + bin=0; + assert(bin>=0 && binfirst < f2it->first) + { + f1it = vFeatVec1.lower_bound(f2it->first); + } + else + { + f2it = vFeatVec2.lower_bound(f1it->first); + } + } + + if(mbCheckOrientation) + { + int ind1=-1; + int ind2=-1; + int ind3=-1; + + ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3); + + for(int i=0; i > &vMatchedPairs, const bool bOnlyStereo, vector &vMatchedPoints) { @@ -1620,8 +1864,6 @@ int ORBmatcher::Fuse(KeyFrame *pKF, const vector &vpMapPoints, const continue; } // 地图点无效 或 已经是该帧的地图点（无需融合），跳过 - /*if(pMP->isBad() || pMP->IsInKeyFrame(pKF)) - continue;*/ if(pMP->isBad()) { count_bad++; @@ -1792,16 +2034,6 @@ int ORBmatcher::Fuse(KeyFrame *pKF, const vector &vpMapPoints, const } - /*cout << "count_notMP = " << count_notMP << endl; - cout << "count_bad = " << count_bad << endl; - cout << "count_isinKF = " << count_isinKF << endl; - cout << "count_negdepth = " << count_negdepth << endl; - cout << "count_notinim = " << count_notinim << endl; - cout << "count_dist = " << count_dist << endl; - cout << "count_normal = " << count_normal << endl; - cout << "count_notidx = " << count_notidx << endl; - cout << "count_thcheck = " << count_thcheck << endl; - cout << "tot fused points: " << nFused << endl;*/ return nFused; } diff --git a/src/Optimizer.cc b/src/Optimizer.cc index 53c39b4..15b91ed 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -168,7 +168,6 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vectormnId>maxKFid) maxKFid=pKF->mnId; - //cout << "KF id: " << pKF->mnId << endl; } // 卡方分布 95% 以上可信度的时候的阈值 @@ -349,7 +348,6 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector &vpKFs, const vectorfixed()) - { - //cout << "KF " << pKF->mnId << ": " << endl; - pKF->mHessianPose = cv::Mat(6, 6, CV_64F); - pKF->mbHasHessian = true; - for(int r=0; r<6; ++r) - { - for(int c=0; c<6; ++c) - { - //cout << vSE3->hessian(r, c) << ", "; - pKF->mHessianPose.at(r, c) = vSE3->hessian(r, c); - } - //cout << endl; - } - }*/ - // 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用 - + // 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用 pKF->mTcwGBA.create(4,4,CV_32F); Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA); pKF->mnBAGlobalForKF = nLoopKF; @@ -461,13 +443,9 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vectormnId) + " had been moved " + to_string(dist) + " meters", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--Number of observations: " + to_string(numMonoOptPoints) + " in mono and " + to_string(numStereoOptPoints) + " in stereo", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--Number of discarded observations: " + to_string(numMonoBadPoints) + " in mono and " + to_string(numStereoBadPoints) + " in stereo", Verbose::VERBOSITY_DEBUG); } } } - Verbose::PrintMess("BA: KFs updated", Verbose::VERBOSITY_DEBUG); // Step 5.2 遍历所有地图点,去除其中没有参与优化过程的地图点 for(size_t i=0; imvKeysRight[i - pFrame->Nleft]; Eigen::Matrix obs; @@ -1274,7 +1251,6 @@ int Optimizer::PoseOptimization(Frame *pFrame) cv::Mat pose = Converter::toCvMat(SE3quat_recov); pFrame->SetPose(pose); - //cout << "[PoseOptimization]: initial correspondences-> " << nInitialCorrespondences << " --- outliers-> " << nBad << endl; // 并且返回内点数目 return nInitialCorrespondences-nBad; } @@ -1369,8 +1345,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vector::iterator lit=lLocalKeyFrames.begin(); int lowerId = pKF->mnId; KeyFrame* pLowerKf; @@ -1409,10 +1383,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorIsInertial()) solver->setUserLambdaInit(100.0); // TODO uncomment - //cout << "LM-LBA: lambda init: " << solver->userLambdaInit() << endl; optimizer.setAlgorithm(solver); optimizer.setVerbose(false); @@ -1470,9 +1442,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectormnId; } - Verbose::PrintMess("LM-LBA: opt/fixed KFs: " + to_string(lLocalKeyFrames.size()) + "/" + to_string(lFixedCameras.size()), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("LM-LBA: local MPs: " + to_string(lLocalMapPoints.size()), Verbose::VERBOSITY_DEBUG); - // Set MapPoint vertices // Step 7 添加待优化的3D地图点顶点 // 边的数目 = pose数目 * 地图点数目 @@ -1656,15 +1625,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorchi2()>5.991 || !e->isDepthPositive()) { - //e->setLevel(1); nMonoBadObs++; } - - //e->setRobustKernel(0); } int nBodyBadObs = 0; @@ -1708,11 +1666,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorchi2()>5.991 || !e->isDepthPositive()) { - //e->setLevel(1); nBodyBadObs++; } - - //e->setRobustKernel(0); } // 对于所有的双目的误差边也都进行类似的操作 @@ -1727,19 +1682,12 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorchi2()>7.815 || !e->isDepthPositive()) { - //e->setLevel(1); nStereoBadObs++; } - - //e->setRobustKernel(0); } - Verbose::PrintMess("LM-LBA: First optimization has " + to_string(nMonoBadObs) + " monocular and " + to_string(nStereoBadObs) + " stereo bad observations", Verbose::VERBOSITY_DEBUG); - // Optimize again without the outliers + // Optimize again // Step 11：排除误差较大的outlier后再次优化 -- 第二阶段优化 - //Verbose::PrintMess("LM-LBA: second optimization", Verbose::VERBOSITY_DEBUG); - //optimizer.initializeOptimization(0); - //numPerform_it = optimizer.optimize(10); numPerform_it += optimizer.optimize(5); } @@ -1763,7 +1711,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorchi2()>5.991 || !e->isDepthPositive()) { - // outlier KeyFrame* pKFi = vpEdgeKFMono[i]; vToErase.push_back(make_pair(pKFi,pMP)); } @@ -1799,8 +1746,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorSetPose(Converter::toCvMat(SE3quat)); pKFi->mnNumberOfOpt += numPerform_it; - //cout << "LM-LBA: KF " << pKFi->mnId << " had performed " << pKFi->mnNumberOfOpt << " iterations" << endl; if(pKFi->mnNumberOfOpt < 10) { vpNonEnoughOptKFs.push_back(pKFi); @@ -1859,9 +1801,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, vectorIncreaseChangeIndex(); } -void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap, int& num_fixedKF) +void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges) { - //cout << "LBA" << endl; // Local KeyFrames: First Breath Search from Current Keyframe list lLocalKeyFrames; @@ -1905,6 +1846,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } } } + num_MPs = lLocalMapPoints.size(); // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes list lFixedCameras; @@ -1926,8 +1868,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap num_fixedKF = lFixedCameras.size() + num_fixedKF; if(num_fixedKF < 2) { - //Verbose::PrintMess("LM-LBA: New Fixed KFs had been set", Verbose::VERBOSITY_NORMAL); - //TODO We set 2 KFs to fixed to avoid a degree of freedom in scale list::iterator lit=lLocalKeyFrames.begin(); int lowerId = pKF->mnId; KeyFrame* pLowerKf; @@ -1966,10 +1906,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap if(num_fixedKF == 0) { - Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_NORMAL); - //return; + Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_QUIET); + return; } - //Verbose::PrintMess("LM-LBA: There are " + to_string(lLocalKeyFrames.size()) + " KFs and " + to_string(lLocalMapPoints.size()) + " MPs to optimize. " + to_string(num_fixedKF) + " KFs are fixed", Verbose::VERBOSITY_DEBUG); // Setup optimizer g2o::SparseOptimizer optimizer; @@ -1981,8 +1920,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); if (pMap->IsInertial()) - solver->setUserLambdaInit(100.0); // TODO uncomment - //cout << "LM-LBA: lambda init: " << solver->userLambdaInit() << endl; + solver->setUserLambdaInit(100.0); optimizer.setAlgorithm(solver); optimizer.setVerbose(false); @@ -2004,7 +1942,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap if(pKFi->mnId>maxKFid) maxKFid=pKFi->mnId; } - //Verbose::PrintMess("LM-LBA: KFs to optimize added", Verbose::VERBOSITY_DEBUG); + num_OptKF = lLocalKeyFrames.size(); // Set Fixed KeyFrame vertices for(list::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++) @@ -2177,8 +2115,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } } } - - //Verbose::PrintMess("LM-LBA: total observations: " + to_string(vpMapPointEdgeMono.size()+vpMapPointEdgeStereo.size()), Verbose::VERBOSITY_DEBUG); + num_edges = nEdges; if(pbStopFlag) if(*pbStopFlag) @@ -2190,9 +2127,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap optimizer.optimize(5); std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); - //std::cout << "LBA time = " << std::chrono::duration_cast(end - begin).count() << "[ms]" << std::endl; - //std::cout << "Keyframes: " << nKFs << " --- MapPoints: " << nPoints << " --- Edges: " << nEdges << endl; - bool bDoMore= true; if(pbStopFlag) @@ -2214,11 +2148,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap if(e->chi2()>5.991 || !e->isDepthPositive()) { - // e->setLevel(1); // MODIFICATION nMonoBadObs++; } - - //e->setRobustKernel(0); } int nBodyBadObs = 0; @@ -2232,11 +2163,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap if(e->chi2()>5.991 || !e->isDepthPositive()) { - //e->setLevel(1); nBodyBadObs++; } - - //e->setRobustKernel(0); } int nStereoBadObs = 0; @@ -2250,16 +2178,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap if(e->chi2()>7.815 || !e->isDepthPositive()) { - //TODO e->setLevel(1); nStereoBadObs++; } - - //TODO e->setRobustKernel(0); } - //Verbose::PrintMess("LM-LBA: First optimization has " + to_string(nMonoBadObs) + " monocular and " + to_string(nStereoBadObs) + " stereo bad observations", Verbose::VERBOSITY_DEBUG); - // Optimize again without the outliers - //Verbose::PrintMess("LM-LBA: second optimization", Verbose::VERBOSITY_DEBUG); + // Optimize again optimizer.initializeOptimization(0); optimizer.optimize(10); @@ -2314,39 +2237,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } } - //Verbose::PrintMess("LM-LBA: outlier observations: " + to_string(vToErase.size()), Verbose::VERBOSITY_DEBUG); - bool bRedrawError = false; - if(vToErase.size() >= (vpMapPointEdgeMono.size()+vpMapPointEdgeStereo.size()) * 0.5) - { - Verbose::PrintMess("LM-LBA: ERROR IN THE OPTIMIZATION, MOST OF THE POINTS HAS BECOME OUTLIERS", Verbose::VERBOSITY_NORMAL); - - return; - bRedrawError = true; - string folder_name = "test_LBA"; - string name = "_PreLM_LBA"; - name = "_PreLM_LBA_Fixed"; - - } - - // Get Map Mutex unique_lock lock(pMap->mMutexMapUpdate); if(!vToErase.empty()) { - map mspInitialConnectedKFs; - map mspInitialObservationKFs; - if(bRedrawError) - { - for(KeyFrame* pKFi : lLocalKeyFrames) - { - - mspInitialConnectedKFs[pKFi] = pKFi->GetConnectedKeyFrames().size(); - mspInitialObservationKFs[pKFi] = pKFi->GetNumberMPs(); - } - } - - //cout << "LM-LBA: There are " << vToErase.size() << " observations whose will be deleted from the map" << endl; for(size_t i=0;iEraseMapPointMatch(pMPi); pMPi->EraseObservation(pKFi); } - - map mspFinalConnectedKFs; - map mspFinalObservationKFs; - if(bRedrawError) - { - ofstream f_lba; - f_lba.open("test_LBA/LBA_failure_KF" + to_string(pKF->mnId) +".txt"); - f_lba << "# KF id, Initial Num CovKFs, Final Num CovKFs, Initial Num MPs, Fimal Num MPs" << endl; - f_lba << fixed; - - for(KeyFrame* pKFi : lLocalKeyFrames) - { - pKFi->UpdateConnections(); - int finalNumberCovKFs = pKFi->GetConnectedKeyFrames().size(); - int finalNumberMPs = pKFi->GetNumberMPs(); - f_lba << pKFi->mnId << ", " << mspInitialConnectedKFs[pKFi] << ", " << finalNumberCovKFs << ", " << mspInitialObservationKFs[pKFi] << ", " << finalNumberMPs << endl; - - mspFinalConnectedKFs[pKFi] = finalNumberCovKFs; - mspFinalObservationKFs[pKFi] = finalNumberMPs; - } - - f_lba.close(); - } } // Recover optimized data @@ -2387,62 +2259,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap KeyFrame* pKFi = *lit; g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); - cv::Mat Tiw = Converter::toCvMat(SE3quat); - cv::Mat Tco_cn = pKFi->GetPose() * Tiw.inv(); - cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); - double dist = cv::norm(trasl); pKFi->SetPose(Converter::toCvMat(SE3quat)); - if(dist > 1.0) - { - bShowStats = true; - Verbose::PrintMess("LM-LBA: Too much distance in KF " + to_string(pKFi->mnId) + ", " + to_string(dist) + " meters. Current KF " + to_string(pKF->mnId), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("LM-LBA: Number of connections between the KFs " + to_string(pKF->GetWeight((pKFi))), Verbose::VERBOSITY_DEBUG); - - int numMonoMP = 0, numBadMonoMP = 0; - int numStereoMP = 0, numBadStereoMP = 0; - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) - continue; - - if(e->chi2()>5.991 || !e->isDepthPositive()) - { - numBadMonoMP++; - } - else - { - numMonoMP++; - } - } - - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) - continue; - - if(e->chi2()>7.815 || !e->isDepthPositive()) - { - numBadStereoMP++; - } - else - { - numStereoMP++; - } - } - Verbose::PrintMess("LM-LBA: Good observations in mono " + to_string(numMonoMP) + " and stereo " + to_string(numStereoMP), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("LM-LBA: Bad observations in mono " + to_string(numBadMonoMP) + " and stereo " + to_string(numBadStereoMP), Verbose::VERBOSITY_DEBUG); - } } //Points @@ -2454,15 +2272,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap pMP->UpdateNormalAndDepth(); } - if(bRedrawError) - { - string folder_name = "test_LBA"; - string name = "_PostLM_LBA"; - //pMap->printReprojectionError(lLocalKeyFrames, pKF, name, folder_name); - name = "_PostLM_LBA_Fixed"; - //pMap->printReprojectionError(lFixedCameras, pKF, name, folder_name); - } - // TODO Check this changeindex pMap->IncreaseChangeIndex(); } @@ -2735,15 +2544,8 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p count_kf++; count_imu++; } - /*if(count_kf<3) - cout << "EG: kf with " << count_kf << " edges!! ID: " << pKF->mnId << endl;*/ } - //cout << "EG: Number of KFs: " << vpKFs.size() << endl; - //cout << "EG: spaning tree edges: " << count_spa_tree << endl; - //cout << "EG: Loop edges: " << count_loop << endl; - //cout << "EG: covisible edges: " << count_cov << endl; - //cout << "EG: imu edges: " << count_imu << endl; // Optimize! optimizer.initializeOptimization(); optimizer.computeActiveErrors(); @@ -2751,7 +2553,6 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p optimizer.optimize(20); optimizer.computeActiveErrors(); float errEnd = optimizer.activeRobustChi2(); - //cout << "err_0/err_end: " << err0 << "/" << errEnd << endl; unique_lock lock(pMap->mMutexMapUpdate); // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] @@ -2773,7 +2574,6 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p cv::Mat Tiw = Converter::toCvSE3(eigR,eigt); pKFi->SetPose(Tiw); - // cout << "angle KF " << nIDi << ": " << (180.0/3.1415)*acos(vZvectors[nIDi].dot(eigR*z_vec)) << endl; } @@ -2810,18 +2610,12 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p pMP->UpdateNormalAndDepth(); } - // TODO Check this changeindex pMap->IncreaseChangeIndex(); } void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, vector &vpNonFixedKFs, vector &vpNonCorrectedMPs, double scale) { - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedKFs.size()) + " KFs fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedCorrectedKFs.size()) + " KFs fixed in the old map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonFixedKFs.size()) + " KFs non-fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonCorrectedMPs.size()) + " MPs non-corrected in the merged map", Verbose::VERBOSITY_DEBUG); - g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); g2o::BlockSolver_6_3::LinearSolverType * linearSolver = @@ -2857,21 +2651,19 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation()); g2o::SE3Quat Siw(Rcw,tcw); vScw[nIDi] = Siw; - vCorrectedSwc[nIDi]=Siw.inverse(); // This KFs mustn't be corrected + vCorrectedSwc[nIDi]=Siw.inverse(); VSE3->setEstimate(Siw); VSE3->setFixed(true); VSE3->setId(nIDi); VSE3->setMarginalized(false); - //VSim3->_fix_scale = true; //TODO vbFromOtherMap[nIDi] = false; optimizer.addVertex(VSE3); vpVertices[nIDi]=VSE3; } - cout << "Opt_Essential: vpFixedKFs loaded" << endl; set sIdKF; for(KeyFrame* pKFi : vpFixedCorrectedKFs) @@ -2908,7 +2700,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & sIdKF.insert(nIDi); } - Verbose::PrintMess("Opt_Essential: vpFixedCorrectedKFs loaded", Verbose::VERBOSITY_DEBUG); for(KeyFrame* pKFi : vpNonFixedKFs) { @@ -2922,9 +2713,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & g2o::VertexSE3Expmap* VSE3 = new g2o::VertexSE3Expmap(); - //cv::Mat Tcw = pKFi->mTcwBefMerge; - //Eigen::Matrix Rcw = Converter::toMatrix3d(Tcw.rowRange(0,3).colRange(0,3)); - //Eigen::Matrix tcw = Converter::toVector3d(Tcw.rowRange(0,3).col(3)); Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation()); Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation()) / scale; g2o::SE3Quat Siw(Rcw,tcw); @@ -2935,7 +2723,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & VSE3->setId(nIDi); VSE3->setMarginalized(false); - //VSim3->_fix_scale = true; vbFromOtherMap[nIDi] = true; optimizer.addVertex(VSE3); @@ -2945,7 +2732,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & sIdKF.insert(nIDi); } - Verbose::PrintMess("Opt_Essential: vpNonFixedKFs loaded", Verbose::VERBOSITY_DEBUG); vector vpKFs; vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size()); @@ -2954,8 +2740,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & vpKFs.insert(vpKFs.end(),vpNonFixedKFs.begin(),vpNonFixedKFs.end()); set spKFs(vpKFs.begin(), vpKFs.end()); - Verbose::PrintMess("Opt_Essential: List of KF loaded", Verbose::VERBOSITY_DEBUG); - const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); for(KeyFrame* pKFi : vpKFs) @@ -2969,17 +2753,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & { Swi_bef = vScw_bef[nIDi].inverse(); } - /*if(pKFi->mnMergeCorrectedForKF == pCurKF->mnId) - { - Swi = vScw[nIDi].inverse(); - } - else - { - cv::Mat Twi = pKFi->mTwcBefMerge; - Swi = g2o::Sim3(Converter::toMatrix3d(Twi.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Twi.rowRange(0, 3).col(3)),1.0); - }*/ - KeyFrame* pParentKFi = pKFi->GetParent(); @@ -2995,17 +2768,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & Sjw_bef = vScw_bef[nIDj]; } - /*if(pParentKFi->mnMergeCorrectedForKF == pCurKF->mnId) - { - Sjw = vScw[nIDj]; - } - else - { - cv::Mat Tjw = pParentKFi->mTcwBefMerge; - Sjw = g2o::Sim3(Converter::toMatrix3d(Tjw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tjw.rowRange(0, 3).col(3)),1.0); - }*/ - g2o::SE3Quat Sji; if(vbFromOtherMap[nIDi] && vbFromOtherMap[nIDj]) @@ -3041,17 +2803,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & Slw_bef = vScw_bef[pLKF->mnId]; } - /*if(pLKF->mnMergeCorrectedForKF == pCurKF->mnId) - { - Slw = vScw[pLKF->mnId]; - } - else - { - cv::Mat Tlw = pLKF->mTcwBefMerge; - Slw = g2o::Sim3(Converter::toMatrix3d(Tlw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tlw.rowRange(0, 3).col(3)),1.0); - }*/ - g2o::SE3Quat Sli; if(vbFromOtherMap[nIDi] && vbFromOtherMap[pLKF->mnId]) @@ -3089,16 +2840,6 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & { Snw_bef = vScw_bef[pKFn->mnId]; } - /*if(pKFn->mnMergeCorrectedForKF == pCurKF->mnId) - { - Snw = vScw[pKFn->mnId]; - } - else - { - cv::Mat Tnw = pKFn->mTcwBefMerge; - Snw = g2o::Sim3(Converter::toMatrix3d(Tnw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tnw.rowRange(0, 3).col(3)),1.0); - }*/ g2o::SE3Quat Sni; @@ -3121,22 +2862,14 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & } } } - - if(num_connections == 0 ) - { - Verbose::PrintMess("Opt_Essential: KF " + to_string(pKFi->mnId) + " has 0 connections", Verbose::VERBOSITY_DEBUG); - } } // Optimize! optimizer.initializeOptimization(); optimizer.optimize(20); - Verbose::PrintMess("Opt_Essential: Finish the optimization", Verbose::VERBOSITY_DEBUG); - unique_lock lock(pMap->mMutexMapUpdate); - Verbose::PrintMess("Opt_Essential: Apply the new pose to the KFs", Verbose::VERBOSITY_DEBUG); // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] for(KeyFrame* pKFi : vpNonFixedKFs) { @@ -3156,60 +2889,17 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & cv::Mat Tiw = Converter::toCvSE3(eigR,eigt); - /*{ - cv::Mat Tco_cn = pKFi->GetPose() * Tiw.inv(); - cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); - double dist = cv::norm(trasl); - if(dist > 1.0) - { - cout << "--Distance: " << dist << " meters" << endl; - cout << "--To much distance correction in EssentGraph: It has connected " << pKFi->GetVectorCovisibleKeyFrames().size() << " KFs" << endl; - } - - string strNameFile = pKFi->mNameFile; - cv::Mat imLeft = cv::imread(strNameFile, CV_LOAD_IMAGE_UNCHANGED); - - cv::cvtColor(imLeft, imLeft, CV_GRAY2BGR); - - vector vpMapPointsKFi = pKFi->GetMapPointMatches(); - for(int j=0; jisBad()) - { - continue; - } - string strNumOBs = to_string(vpMapPointsKFi[j]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[j].pt, 2, cv::Scalar(0, 255, 0)); - cv::putText(imLeft, strNumOBs, pKFi->mvKeys[j].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - } - - string namefile = "./test_OptEssent/Essent_" + to_string(pCurKF->mnId) + "_KF" + to_string(pKFi->mnId) +"_D" + to_string(dist) +".png"; - cv::imwrite(namefile, imLeft); - }*/ - pKFi->mTcwBefMerge = pKFi->GetPose(); pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); pKFi->SetPose(Tiw); } - Verbose::PrintMess("Opt_Essential: Apply the new pose to the MPs", Verbose::VERBOSITY_DEBUG); - cout << "Opt_Essential: number of points -> " << vpNonCorrectedMPs.size() << endl; // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose for(MapPoint* pMPi : vpNonCorrectedMPs) { if(pMPi->isBad()) continue; - //Verbose::PrintMess("Opt_Essential: MP id " + to_string(pMPi->mnId), Verbose::VERBOSITY_DEBUG); - /*int nIDr; - if(pMPi->mnCorrectedByKF==pCurKF->mnId) - { - nIDr = pMPi->mnCorrectedReference; - } - else - { - - }*/ KeyFrame* pRefKF = pMPi->GetReferenceKeyFrame(); g2o::SE3Quat Srw; g2o::SE3Quat correctedSwr; @@ -3224,51 +2914,30 @@ void Optimizer::OptimizeEssentialGraph6DoF(KeyFrame* pCurKF, vector & pMPi->EraseObservation(pRefKF); pRefKF = pMPi->GetReferenceKeyFrame(); } - /*if(pRefKF->mnMergeCorrectedForKF == pCurKF->mnId) - { - int nIDr = pRefKF->mnId; - Srw = vScw[nIDr]; - correctedSwr = vCorrectedSwc[nIDr]; - } - else - {*/ - //cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge; - //Eigen::Matrix RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0,3).colRange(0,3)); - //Eigen::Matrix tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0,3).col(3)); - Srw = vScw_bef[pRefKF->mnId]; //g2o::SE3Quat(RNonCorrectedwr,tNonCorrectedwr).inverse(); + Srw = vScw_bef[pRefKF->mnId]; //g2o::SE3Quat(RNonCorrectedwr,tNonCorrectedwr).inverse(); + + cv::Mat Twr = pRefKF->GetPoseInverse(); + Eigen::Matrix Rwr = Converter::toMatrix3d(Twr.rowRange(0,3).colRange(0,3)); + Eigen::Matrix twr = Converter::toVector3d(Twr.rowRange(0,3).col(3)); + correctedSwr = g2o::SE3Quat(Rwr,twr); - cv::Mat Twr = pRefKF->GetPoseInverse(); - Eigen::Matrix Rwr = Converter::toMatrix3d(Twr.rowRange(0,3).colRange(0,3)); - Eigen::Matrix twr = Converter::toVector3d(Twr.rowRange(0,3).col(3)); - correctedSwr = g2o::SE3Quat(Rwr,twr); - //} - //cout << "Opt_Essential: Loaded the KF reference position" << endl; cv::Mat P3Dw = pMPi->GetWorldPos() / scale; Eigen::Matrix eigP3Dw = Converter::toVector3d(P3Dw); Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); - //cout << "Opt_Essential: Calculated the new MP position" << endl; cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw); - //cout << "Opt_Essential: Converted the position to the OpenCV format" << endl; pMPi->SetWorldPos(cvCorrectedP3Dw); - //cout << "Opt_Essential: Loaded the corrected position in the MP object" << endl; pMPi->UpdateNormalAndDepth(); } - Verbose::PrintMess("Opt_Essential: End of the optimization", Verbose::VERBOSITY_DEBUG); } void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, vector &vpNonFixedKFs, vector &vpNonCorrectedMPs) { - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedKFs.size()) + " KFs fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedCorrectedKFs.size()) + " KFs fixed in the old map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonFixedKFs.size()) + " KFs non-fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonCorrectedMPs.size()) + " MPs non-corrected in the merged map", Verbose::VERBOSITY_DEBUG); - g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); g2o::BlockSolver_7_3::LinearSolverType * linearSolver = @@ -3309,7 +2978,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi VSim3->setId(nIDi); VSim3->setMarginalized(false); - VSim3->_fix_scale = true; //TODO + VSim3->_fix_scale = true; optimizer.addVertex(VSim3); @@ -3330,7 +2999,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation()); Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation()); g2o::Sim3 Siw(Rcw,tcw,1.0); - //vScw[nIDi] = Siw; vCorrectedSwc[nIDi]=Siw.inverse(); // This KFs mustn't be corrected VSim3->setEstimate(Siw); @@ -3364,9 +3032,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); - //cv::Mat Tcw = pKFi->mTcwBefMerge; - //Eigen::Matrix Rcw = Converter::toMatrix3d(Tcw.rowRange(0,3).colRange(0,3)); - //Eigen::Matrix tcw = Converter::toVector3d(Tcw.rowRange(0,3).col(3)); Eigen::Matrix Rcw = Converter::toMatrix3d(pKFi->GetRotation()); Eigen::Matrix tcw = Converter::toVector3d(pKFi->GetTranslation()); g2o::Sim3 Siw(Rcw,tcw,1.0); @@ -3404,17 +3069,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi const int nIDi = pKFi->mnId; g2o::Sim3 Swi = vScw[nIDi].inverse(); - /*if(pKFi->mnMergeCorrectedForKF == pCurKF->mnId) - { - Swi = vScw[nIDi].inverse(); - } - else - { - cv::Mat Twi = pKFi->mTwcBefMerge; - Swi = g2o::Sim3(Converter::toMatrix3d(Twi.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Twi.rowRange(0, 3).col(3)),1.0); - }*/ - KeyFrame* pParentKFi = pKFi->GetParent(); @@ -3425,17 +3079,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi g2o::Sim3 Sjw = vScw[nIDj]; - /*if(pParentKFi->mnMergeCorrectedForKF == pCurKF->mnId) - { - Sjw = vScw[nIDj]; - } - else - { - cv::Mat Tjw = pParentKFi->mTcwBefMerge; - Sjw = g2o::Sim3(Converter::toMatrix3d(Tjw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tjw.rowRange(0, 3).col(3)),1.0); - }*/ - g2o::Sim3 Sji = Sjw * Swi; g2o::EdgeSim3* e = new g2o::EdgeSim3(); @@ -3457,17 +3100,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi { g2o::Sim3 Slw = vScw[pLKF->mnId]; - /*if(pLKF->mnMergeCorrectedForKF == pCurKF->mnId) - { - Slw = vScw[pLKF->mnId]; - } - else - { - cv::Mat Tlw = pLKF->mTcwBefMerge; - Slw = g2o::Sim3(Converter::toMatrix3d(Tlw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tlw.rowRange(0, 3).col(3)),1.0); - }*/ - g2o::Sim3 Sli = Slw * Swi; g2o::EdgeSim3* el = new g2o::EdgeSim3(); el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); @@ -3490,16 +3122,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi { g2o::Sim3 Snw = vScw[pKFn->mnId]; - /*if(pKFn->mnMergeCorrectedForKF == pCurKF->mnId) - { - Snw = vScw[pKFn->mnId]; - } - else - { - cv::Mat Tnw = pKFn->mTcwBefMerge; - Snw = g2o::Sim3(Converter::toMatrix3d(Tnw.rowRange(0, 3).colRange(0, 3)), - Converter::toVector3d(Tnw.rowRange(0, 3).col(3)),1.0); - }*/ g2o::Sim3 Sni = Snw * Swi; @@ -3513,22 +3135,15 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi } } } - - if(num_connections == 0 ) - { - Verbose::PrintMess("Opt_Essential: KF " + to_string(pKFi->mnId) + " has 0 connections", Verbose::VERBOSITY_DEBUG); - } } // Optimize! optimizer.initializeOptimization(); optimizer.optimize(20); - Verbose::PrintMess("Opt_Essential: Finish the optimization", Verbose::VERBOSITY_DEBUG); unique_lock lock(pMap->mMutexMapUpdate); - Verbose::PrintMess("Opt_Essential: Apply the new pose to the KFs", Verbose::VERBOSITY_DEBUG); // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] for(KeyFrame* pKFi : vpNonFixedKFs) { @@ -3548,54 +3163,17 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi cv::Mat Tiw = Converter::toCvSE3(eigR,eigt); - /*{ - cv::Mat Tco_cn = pKFi->GetPose() * Tiw.inv(); - cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); - double dist = cv::norm(trasl); - if(dist > 1.0) - { - cout << "--Distance: " << dist << " meters" << endl; - cout << "--To much distance correction in EssentGraph: It has connected " << pKFi->GetVectorCovisibleKeyFrames().size() << " KFs" << endl; - } - string strNameFile = pKFi->mNameFile; - cv::Mat imLeft = cv::imread(strNameFile, CV_LOAD_IMAGE_UNCHANGED); - cv::cvtColor(imLeft, imLeft, CV_GRAY2BGR); - vector vpMapPointsKFi = pKFi->GetMapPointMatches(); - for(int j=0; jisBad()) - { - continue; - } - string strNumOBs = to_string(vpMapPointsKFi[j]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[j].pt, 2, cv::Scalar(0, 255, 0)); - cv::putText(imLeft, strNumOBs, pKFi->mvKeys[j].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - } - string namefile = "./test_OptEssent/Essent_" + to_string(pCurKF->mnId) + "_KF" + to_string(pKFi->mnId) +"_D" + to_string(dist) +".png"; - cv::imwrite(namefile, imLeft); - }*/ - pKFi->mTcwBefMerge = pKFi->GetPose(); pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); pKFi->SetPose(Tiw); } - Verbose::PrintMess("Opt_Essential: Apply the new pose to the MPs", Verbose::VERBOSITY_DEBUG); // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose for(MapPoint* pMPi : vpNonCorrectedMPs) { if(pMPi->isBad()) continue; - Verbose::PrintMess("Opt_Essential: MP id " + to_string(pMPi->mnId), Verbose::VERBOSITY_DEBUG); - /*int nIDr; - if(pMPi->mnCorrectedByKF==pCurKF->mnId) - { - nIDr = pMPi->mnCorrectedReference; - } - else - { - }*/ KeyFrame* pRefKF = pMPi->GetReferenceKeyFrame(); g2o::Sim3 Srw; g2o::Sim3 correctedSwr; @@ -3610,40 +3188,26 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFi pMPi->EraseObservation(pRefKF); pRefKF = pMPi->GetReferenceKeyFrame(); } - /*if(pRefKF->mnMergeCorrectedForKF == pCurKF->mnId) - { - int nIDr = pRefKF->mnId; - Srw = vScw[nIDr]; - correctedSwr = vCorrectedSwc[nIDr]; - } - else - {*/ - cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge; - Eigen::Matrix RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0,3).colRange(0,3)); - Eigen::Matrix tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0,3).col(3)); - Srw = g2o::Sim3(RNonCorrectedwr,tNonCorrectedwr,1.0).inverse(); - cv::Mat Twr = pRefKF->GetPoseInverse(); - Eigen::Matrix Rwr = Converter::toMatrix3d(Twr.rowRange(0,3).colRange(0,3)); - Eigen::Matrix twr = Converter::toVector3d(Twr.rowRange(0,3).col(3)); - correctedSwr = g2o::Sim3(Rwr,twr,1.0); - //} - //cout << "Opt_Essential: Loaded the KF reference position" << endl; + cv::Mat TNonCorrectedwr = pRefKF->mTwcBefMerge; + Eigen::Matrix RNonCorrectedwr = Converter::toMatrix3d(TNonCorrectedwr.rowRange(0,3).colRange(0,3)); + Eigen::Matrix tNonCorrectedwr = Converter::toVector3d(TNonCorrectedwr.rowRange(0,3).col(3)); + Srw = g2o::Sim3(RNonCorrectedwr,tNonCorrectedwr,1.0).inverse(); + + cv::Mat Twr = pRefKF->GetPoseInverse(); + Eigen::Matrix Rwr = Converter::toMatrix3d(Twr.rowRange(0,3).colRange(0,3)); + Eigen::Matrix twr = Converter::toVector3d(Twr.rowRange(0,3).col(3)); + correctedSwr = g2o::Sim3(Rwr,twr,1.0); cv::Mat P3Dw = pMPi->GetWorldPos(); Eigen::Matrix eigP3Dw = Converter::toVector3d(P3Dw); Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); - //cout << "Opt_Essential: Calculated the new MP position" << endl; cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw); - //cout << "Opt_Essential: Converted the position to the OpenCV format" << endl; pMPi->SetWorldPos(cvCorrectedP3Dw); - //cout << "Opt_Essential: Loaded the corrected position in the MP object" << endl; pMPi->UpdateNormalAndDepth(); } - - Verbose::PrintMess("Opt_Essential: End of the optimization", Verbose::VERBOSITY_DEBUG); } void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, @@ -3671,7 +3235,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector > vCorrectedSwc(nMaxKFid+1); vector vpVertices(nMaxKFid+1); - const int minFeat = 100; // TODO Check. originally 100 + const int minFeat = 100; // Set KeyFrame vertices for(size_t i=0, iend=vpKFs.size(); isetId(nIDi); VSim3->setMarginalized(false); - // TODO Check - // VSim3->_fix_scale = bFixScale; optimizer.addVertex(VSim3); @@ -3833,7 +3395,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, } } - Verbose::PrintMess("edges pose graph: " + to_string(count_edges[0]) + ", " + to_string(count_edges[1]) + ", " + to_string(count_edges[2]), Verbose::VERBOSITY_DEBUG); // Optimize! // Step 5：开始g2o优化 optimizer.initializeOptimization(); @@ -3907,7 +3468,6 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, pMP->UpdateNormalAndDepth(); } // 使用相对位姿变换的方法来更新地图点的位姿 - // TODO Check this changeindex pMap->IncreaseChangeIndex(); } @@ -4212,15 +3772,12 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & const int id2 = 2*(i+1); const int i2 = get<0>(pMP2->GetIndexInKeyFrame(pKF2)); - /*if(i2 < 0) - cout << "Sim3-OPT: Error, there is a matched which is not find it" << endl;*/ cv::Mat P3D1c; cv::Mat P3D2c; if(pMP1 && pMP2) { - //if(!pMP1->isBad() && !pMP2->isBad() && i2>=0) if(!pMP1->isBad() && !pMP2->isBad()) { g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ(); @@ -4249,7 +3806,7 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & { nMatchWithoutMP++; - //TODO The 3D position in KF1 doesn't exist + //The 3D position in KF1 doesn't exist if(!pMP2->isBad()) { g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ(); @@ -4342,8 +3899,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & vbIsInKF2.push_back(inKF2); } - Verbose::PrintMess("Sim3: There are " + to_string(nCorrespondences) + " matches, " + to_string(nInKF2) + " are in the KF and " + to_string(nOutKF2) + " are in the connected KFs. There are " + to_string(nMatchWithoutMP) + " matches which have not an associate MP", Verbose::VERBOSITY_DEBUG); - // Optimize! optimizer.initializeOptimization(); optimizer.optimize(5); @@ -4380,8 +3935,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & e21->setRobustKernel(0); } - Verbose::PrintMess("Sim3: First Opt -> Correspondences: " + to_string(nCorrespondences) + "; nBad: " + to_string(nBad) + "; nBadOutKF2: " + to_string(nBadOutKF2), Verbose::VERBOSITY_DEBUG); - int nMoreIterations; if(nBad>0) nMoreIterations=10; @@ -4416,15 +3969,12 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & else { nIn++; - //mAcumHessian += e12->GetHessian(); } } // Recover optimized Sim3 - //Verbose::PrintMess("Sim3: Initial seed " + g2oS12, Verbose::VERBOSITY_DEBUG); g2o::VertexSim3Expmap* vSim3_recov = static_cast(optimizer.vertex(0)); g2oS12= vSim3_recov->estimate(); - //Verbose::PrintMess("Sim3: Optimized solution " + g2oS12, Verbose::VERBOSITY_DEBUG); return nIn; } @@ -4449,10 +3999,8 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & // Camera poses const cv::Mat R1w = pKF1->GetRotation(); const cv::Mat t1w = pKF1->GetTranslation(); - Verbose::PrintMess("Extracted rotation and traslation from the first KF ", Verbose::VERBOSITY_DEBUG); const cv::Mat R2w = pKF2->GetRotation(); const cv::Mat t2w = pKF2->GetTranslation(); - Verbose::PrintMess("Extracted rotation and traslation from the second KF ", Verbose::VERBOSITY_DEBUG); // Set Sim3 vertex g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap(); @@ -4625,8 +4173,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & e21->setRobustKernel(0); } - //cout << "Sim3 -> Correspondences: " << nCorrespondences << "; nBad: " << nBad << endl; - int nMoreIterations; if(nBad>0) nMoreIterations=10; @@ -4661,7 +4207,6 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & else { nIn++; - //mAcumHessian += e12->GetHessian(); } } @@ -4673,9 +4218,8 @@ int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector & } -void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool bLarge, bool bRecInit) +void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge, bool bRecInit) { - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); Map* pCurrentMap = pKF->GetMap(); int maxOpt=10; @@ -4820,6 +4364,10 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool // Set Local temporal KeyFrame vertices N=vpOptimizableKFs.size(); + num_fixedKF = 0; + num_OptKF = 0; + num_MPs = 0; + num_edges = 0; for(int i=0; isetFixed(false); optimizer.addVertex(VA); } + num_OptKF++; } // Set Local visual KeyFrame vertices @@ -4854,6 +4403,8 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool VP->setId(pKFi->mnId); VP->setFixed(false); optimizer.addVertex(VP); + + num_OptKF++; } // Set Fixed KeyFrame vertices @@ -4880,6 +4431,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool VA->setFixed(true); optimizer.addVertex(VA); } + num_fixedKF++; } // Create intertial constraints @@ -4948,8 +4500,8 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool InfoG(r,c)=cvInfoG.at(r,c); vegr[i]->setInformation(InfoG); optimizer.addEdge(vegr[i]); + num_edges++; - // cout << "b"; vear[i] = new EdgeAccRW(); vear[i]->setVertex(0,VA1); vear[i]->setVertex(1,VA2); @@ -4961,6 +4513,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool vear[i]->setInformation(InfoA); optimizer.addEdge(vear[i]); + num_edges++; } else cout << "ERROR building inertial edge" << endl; @@ -5009,6 +4562,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool mVisEdges[(*lit)->mnId] = 0; } + num_MPs = lLocalMapPoints.size(); for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { MapPoint* pMP = *lit; @@ -5064,6 +4618,8 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool vpEdgesMono.push_back(e); vpEdgeKFMono.push_back(pKFi); vpMapPointEdgeMono.push_back(pMP); + + num_edges++; } // Stereo-observation else if(leftIndex != -1)// Stereo observation @@ -5095,6 +4651,8 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool vpEdgesStereo.push_back(e); vpEdgeKFStereo.push_back(pKFi); vpMapPointEdgeStereo.push_back(pMP); + + num_edges++; } // Monocular right observation @@ -5129,6 +4687,8 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool vpEdgesMono.push_back(e); vpEdgeKFMono.push_back(pKFi); vpMapPointEdgeMono.push_back(pMP); + + num_edges++; } } } @@ -5143,7 +4703,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool optimizer.initializeOptimization(); optimizer.computeActiveErrors(); - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); + float err = optimizer.activeRobustChi2(); optimizer.optimize(opt_it); // Originally to 2 float err_end = optimizer.activeRobustChi2(); @@ -5151,8 +4711,6 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool optimizer.setForceStopFlag(pbStopFlag); - std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); - vector > vToErase; vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size()); @@ -5194,11 +4752,7 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool // Get Map Mutex and erase outliers unique_lock lock(pMap->mMutexMapUpdate); - - // TODO: Some convergence problems have been detected here - //cout << "err0 = " << err << endl; - //cout << "err_end = " << err_end << endl; - if((2*err < err_end || isnan(err) || isnan(err_end)) && !bLarge) //bGN) + if((2*err < err_end || isnan(err) || isnan(err_end)) && !bLarge) { cout << "FAIL LOCAL-INERTIAL BA!!!!" << endl; return; @@ -5273,17 +4827,6 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, bool pMap->IncreaseChangeIndex(); - std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); - - /*double t_const = std::chrono::duration_cast >(t1 - t0).count(); - double t_opt = std::chrono::duration_cast >(t2 - t1).count(); - double t_rec = std::chrono::duration_cast >(t3 - t2).count(); - /*std::cout << " Construction time: " << t_const << std::endl; - std::cout << " Optimization time: " << t_opt << std::endl; - std::cout << " Recovery time: " << t_rec << std::endl; - std::cout << " Total time: " << t_const+t_opt+t_rec << std::endl; - std::cout << " Optimization iterations: " << opt_it << std::endl;*/ - } Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end) @@ -5540,10 +5083,9 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc std::set setEdges = optimizer.edges(); std::cout << "start optimization" << std::endl; - optimizer.setVerbose(false); optimizer.initializeOptimization(); + optimizer.setVerbose(false); optimizer.optimize(its); - std::cout << "end optimization" << std::endl; scale = VS->estimate(); @@ -5565,8 +5107,6 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc cv::Mat cvbg = Converter::toCvMat(bg); //Keyframes velocities and biases - std::cout << "update Keyframes velocities and biases" << std::endl; - const int N = vpKFs.size(); for(size_t i=0; iSetNewBias(b); - - } } void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA) { - int its = 200; // Check number of iterations + int its = 200; long unsigned int maxKFid = pMap->GetMaxKFid(); const vector vpKFs = pMap->GetAllKeyFrames(); @@ -5611,7 +5149,6 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vect optimizer.setAlgorithm(solver); - // Set KeyFrame vertices (fixed poses and optimizable velocities) for(size_t i=0; i vpKFs, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA) { - int its = 200; // Check number of iterations + int its = 200; long unsigned int maxKFid = vpKFs[0]->GetMap()->GetMaxKFid(); // Setup optimizer @@ -5777,8 +5314,7 @@ void Optimizer::InertialOptimization(vector vpKFs, Eigen::Vector3d &b for(size_t i=0; imnId>maxKFid) - // continue; + VertexPose * VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); @@ -6145,17 +5681,14 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vector>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) { - //cout << "--KF view init" << endl; KeyFrame* pKF = mit->first; if(spKeyFrameBA.find(pKF) == spKeyFrameBA.end() || pKF->isBad() || pKF->mnId>maxKFid || pKF->mnBALocalForKF != pCurrentKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) continue; - //cout << "-- KF view exists" << endl; nEdges++; const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)]; - //cout << "-- KeyPoint loads" << endl; if(pKF->mvuRight[get<0>(mit->second)]<0) //Monocular { @@ -6170,7 +5703,6 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vectorsetMeasurement(obs); const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - //cout << "-- Sigma loads" << endl; g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); @@ -6180,15 +5712,12 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vectorfy = pKF->fy; e->cx = pKF->cx; e->cy = pKF->cy; - //cout << "-- Calibration loads" << endl; optimizer.addEdge(e); - //cout << "-- Edge added" << endl; vpEdgesMono.push_back(e); vpEdgeKFMono.push_back(pKF); vpMapPointEdgeMono.push_back(pMPi); - //cout << "-- Added to vector" << endl; } else // RGBD or Stereo { @@ -6221,12 +5750,9 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vector > vToErase; @@ -6333,7 +5856,6 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vectorEraseObservation(pKFi); } } - //cout << "End to erase observations" << endl; // Recover optimized data @@ -6348,7 +5870,6 @@ void Optimizer::MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vectorSetPose(Converter::toCvMat(SE3quat)); } - //cout << "End to update the KeyFrames" << endl; //Points for(MapPoint* pMPi : vpMPs) @@ -6389,8 +5910,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju Map* pCurrentMap = pMainKF->GetMap(); - //set sNumObsMP; - // Set fixed KeyFrame vertices for(KeyFrame* pKFi : vpFixedKF) { @@ -6421,25 +5940,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju vpMPs.push_back(pMPi); pMPi->mnBALocalForMerge=pMainKF->mnId; } - /*if(sNumObsMP.find(pMPi) == sNumObsMP.end()) - { - sNumObsMP.insert(pMPi); - } - else - { - if(pMPi->mnBALocalForMerge!=pMainKF->mnId) - { - vpMPs.push_back(pMPi); - pMPi->mnBALocalForMerge=pMainKF->mnId; - } - }*/ } spKeyFrameBA.insert(pKFi); } - //cout << "End to load Fixed KFs" << endl; - // Set non fixed Keyframe vertices set spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end()); for(KeyFrame* pKFi : vpAdjustKF) @@ -6463,10 +5968,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju { if(!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) { - /*if(sNumObsMP.find(pMPi) == sNumObsMP.end()) - { - sNumObsMP.insert(pMPi); - }*/ if(pMPi->mnBALocalForMerge != pMainKF->mnId) { vpMPs.push_back(pMPi); @@ -6479,10 +5980,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju spKeyFrameBA.insert(pKFi); } - //Verbose::PrintMess("LBA: There are " + to_string(vpMPs.size()) + " MPs to optimize", Verbose::VERBOSITY_NORMAL); - - //cout << "End to load KFs for position adjust" << endl; - const int nExpectedSize = (vpAdjustKF.size()+vpFixedKF.size())*vpMPs.size(); vector vpEdgesMono; @@ -6529,17 +6026,14 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju //SET EDGES for(map>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) { - //cout << "--KF view init" << endl; KeyFrame* pKF = mit->first; if(pKF->isBad() || pKF->mnId>maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) continue; - //cout << "-- KF view exists" << endl; nEdges++; const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)]; - //cout << "-- KeyPoint loads" << endl; if(pKF->mvuRight[get<0>(mit->second)]<0) //Monocular { @@ -6555,22 +6049,18 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju e->setMeasurement(obs); const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - //cout << "-- Sigma loads" << endl; g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuber2D); e->pCamera = pKF->mpCamera; - //cout << "-- Calibration loads" << endl; optimizer.addEdge(e); - //cout << "-- Edge added" << endl; vpEdgesMono.push_back(e); vpEdgeKFMono.push_back(pKF); vpMapPointEdgeMono.push_back(pMPi); - //cout << "-- Added to vector" << endl; mpObsKFs[pKF]++; } @@ -6608,10 +6098,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju mpObsKFs[pKF]++; } - //cout << "-- End to load point" << endl; } } - //Verbose::PrintMess("LBA: number total of edged -> " + to_string(vpEdgeKFMono.size() + vpEdgeKFStereo.size()), Verbose::VERBOSITY_NORMAL); map mStatsObs; for(map::iterator it = mpObsMPs.begin(); it != mpObsMPs.end(); ++it) @@ -6620,19 +6108,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju int numObs = it->second; mStatsObs[numObs]++; - /*if(numObs < 5) - { - cout << "LBA: MP " << pMPi->mnId << " has " << numObs << " observations" << endl; - }*/ + } - /*for(map::iterator it = mStatsObs.begin(); it != mStatsObs.end(); ++it) - { - cout << "LBA: There are " << it->second << " MPs with " << it->first << " observations" << endl; - }*/ - - //cout << "End to load MPs" << endl; - if(pbStopFlag) if(*pbStopFlag) return; @@ -6640,8 +6118,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju optimizer.initializeOptimization(); optimizer.optimize(5); - //cout << "End the first optimization" << endl; - bool bDoMore= true; if(pbStopFlag) @@ -6689,12 +6165,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } Verbose::PrintMess("LBA: First optimization, there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " sterero bad edges", Verbose::VERBOSITY_DEBUG); - // Optimize again without the outliers + // Optimize again without the outliers - optimizer.initializeOptimization(0); - optimizer.optimize(10); + optimizer.initializeOptimization(0); + optimizer.optimize(10); - //cout << "End the second optimization (without outliers)" << endl; } vector > vToErase; @@ -6795,14 +6270,12 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju const map> observations = pMPi->GetObservations(); for(map>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) { - //cout << "--KF view init" << endl; KeyFrame* pKF = mit->first; if(pKF->isBad() || pKF->mnId>maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) continue; const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)]; - //cout << "-- KeyPoint loads" << endl; if(pKF->mvuRight[get<0>(mit->second)]<0) //Monocular { @@ -6813,12 +6286,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju mpObsFinalKFs[pKF]++; } - //cout << "-- End to load point" << endl; } } - //cout << "End to erase observations" << endl; - // Recover optimized data //Keyframes @@ -6830,9 +6300,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); cv::Mat Tiw = Converter::toCvMat(SE3quat); - cv::Mat Tco_cn = pKFi->GetPose() * Tiw.inv(); - cv::Vec3d trasl = Tco_cn.rowRange(0,3).col(3); - double dist = cv::norm(trasl); int numMonoBadPoints = 0, numMonoOptPoints = 0; int numStereoBadPoints = 0, numStereoOptPoints = 0; @@ -6898,112 +6365,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju { Verbose::PrintMess("LBA ERROR: KF " + to_string(pKFi->mnId) + " has only " + to_string(numMonoOptPoints) + " monocular and " + to_string(numStereoOptPoints) + " stereo points", Verbose::VERBOSITY_DEBUG); } - if(dist > 1.0) - { - if(bShowImages) - { - string strNameFile = pKFi->mNameFile; - cv::Mat imLeft = cv::imread(strNameFile, CV_LOAD_IMAGE_UNCHANGED); - cv::cvtColor(imLeft, imLeft, CV_GRAY2BGR); - - int numPointsMono = 0, numPointsStereo = 0; - int numPointsMonoBad = 0, numPointsStereoBad = 0; - for(int i=0; iisBad()) - { - continue; - } - int index = get<0>(vpMonoMPsOpt[i]->GetIndexInKeyFrame(pKFi)); - if(index < 0) - { - //cout << "LBA ERROR: KF has a monocular observation which is not recognized by the MP" << endl; - //cout << "LBA: KF " << pKFi->mnId << " and MP " << vpMonoMPsOpt[i]->mnId << " with index " << endl; - continue; - } - - //string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[index].pt, 2, cv::Scalar(255, 0, 0)); - //cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - numPointsMono++; - } - - for(int i=0; iisBad()) - { - continue; - } - int index = get<0>(vpStereoMPsOpt[i]->GetIndexInKeyFrame(pKFi)); - if(index < 0) - { - //cout << "LBA: KF has a stereo observation which is not recognized by the MP" << endl; - //cout << "LBA: KF " << pKFi->mnId << " and MP " << vpStereoMPsOpt[i]->mnId << endl; - continue; - } - - //string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[index].pt, 2, cv::Scalar(0, 255, 0)); - //cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - numPointsStereo++; - } - - for(int i=0; iisBad()) - { - continue; - } - int index = get<0>(vpMonoMPsBad[i]->GetIndexInKeyFrame(pKFi)); - if(index < 0) - { - //cout << "LBA ERROR: KF has a monocular observation which is not recognized by the MP" << endl; - //cout << "LBA: KF " << pKFi->mnId << " and MP " << vpMonoMPsOpt[i]->mnId << " with index " << endl; - continue; - } - - //string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[index].pt, 2, cv::Scalar(0, 0, 255)); - //cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - numPointsMonoBad++; - } - for(int i=0; iisBad()) - { - continue; - } - int index = get<0>(vpStereoMPsBad[i]->GetIndexInKeyFrame(pKFi)); - if(index < 0) - { - //cout << "LBA: KF has a stereo observation which is not recognized by the MP" << endl; - //cout << "LBA: KF " << pKFi->mnId << " and MP " << vpStereoMPsOpt[i]->mnId << endl; - continue; - } - - //string strNumOBs = to_string(vpMapPointsKF[i]->Observations()); - cv::circle(imLeft, pKFi->mvKeys[index].pt, 2, cv::Scalar(0, 0, 255)); - //cv::putText(imLeft, strNumOBs, pKF->mvKeys[i].pt, CV_FONT_HERSHEY_DUPLEX, 1, cv::Scalar(255, 0, 0)); - numPointsStereoBad++; - } - - string namefile = "./test_LBA/LBA_KF" + to_string(pKFi->mnId) + "_" + to_string(numPointsMono + numPointsStereo) +"_D" + to_string(dist) +".png"; - cv::imwrite(namefile, imLeft); - - Verbose::PrintMess("--LBA in KF " + to_string(pKFi->mnId), Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--Distance: " + to_string(dist) + " meters", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--Number of observations: " + to_string(numMonoOptPoints) + " in mono and " + to_string(numStereoOptPoints) + " in stereo", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--Number of discarded observations: " + to_string(numMonoBadPoints) + " in mono and " + to_string(numStereoBadPoints) + " in stereo", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--To much distance correction in LBA: It has " + to_string(mpObsKFs[pKFi]) + " observated MPs", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--To much distance correction in LBA: It has " + to_string(mpObsFinalKFs[pKFi]) + " deleted observations", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("--------", Verbose::VERBOSITY_DEBUG); - } - } pKFi->SetPose(Tiw); } - //cout << "End to update the KeyFrames" << endl; //Points for(MapPoint* pMPi : vpMPs) @@ -7016,7 +6381,6 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju pMPi->UpdateNormalAndDepth(); } - //cout << "End to update MapPoint" << endl; } @@ -7029,9 +6393,9 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS vpOptimizableKFs.reserve(2*Nd); // For cov KFS, inertial parameters are not optimized - const int maxCovKF=30; + const int maxCovKF=15; vector vpOptimizableCovKFs; - vpOptimizableCovKFs.reserve(maxCovKF); + vpOptimizableCovKFs.reserve(2*maxCovKF); // Add sliding window for current KF vpOptimizableKFs.push_back(pCurrKF); @@ -7135,31 +6499,41 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS } } - std::vector> pairs; - pairs.reserve(mLocalObs.size()); - for (auto itr = mLocalObs.begin(); itr != mLocalObs.end(); ++itr) - pairs.push_back(*itr); - sort(pairs.begin(), pairs.end(),sortByVal); - - // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes - int i=0; - for(vector>::iterator lit=pairs.begin(), lend=pairs.end(); lit!=lend; lit++, i++) + int i = 0; + const int min_obs = 10; + vector vNeighCurr = pCurrKF->GetCovisiblesByWeight(min_obs); + for(vector::iterator lit=vNeighCurr.begin(), lend=vNeighCurr.end(); lit!=lend; lit++) { - map> observations = lit->first->GetObservations(); if(i>=maxCovKF) break; - for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) - { - KeyFrame* pKFi = mit->first; + KeyFrame* pKFi = *lit; - if(pKFi->mnBALocalForKF!=pCurrKF->mnId && pKFi->mnBAFixedForKF!=pCurrKF->mnId) // If optimizable or already included... + if(pKFi->mnBALocalForKF!=pCurrKF->mnId && pKFi->mnBAFixedForKF!=pCurrKF->mnId) // If optimizable or already included... + { + pKFi->mnBALocalForKF=pCurrKF->mnId; + if(!pKFi->isBad()) { - pKFi->mnBALocalForKF=pCurrKF->mnId; - if(!pKFi->isBad()) - { - vpOptimizableCovKFs.push_back(pKFi); - break; - } + i++; + vpOptimizableCovKFs.push_back(pKFi); + } + } + } + + i = 0; + vector vNeighMerge = pMergeKF->GetCovisiblesByWeight(min_obs); + for(vector::iterator lit=vNeighCurr.begin(), lend=vNeighCurr.end(); lit!=lend; lit++, i++) + { + if(i>=maxCovKF) + break; + KeyFrame* pKFi = *lit; + + if(pKFi->mnBALocalForKF!=pCurrKF->mnId && pKFi->mnBAFixedForKF!=pCurrKF->mnId) // If optimizable or already included... + { + pKFi->mnBALocalForKF=pCurrKF->mnId; + if(!pKFi->isBad()) + { + i++; + vpOptimizableCovKFs.push_back(pKFi); } } } @@ -7172,7 +6546,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - solver->setUserLambdaInit(1e3); // TODO uncomment + solver->setUserLambdaInit(1e3); optimizer.setAlgorithm(solver); optimizer.setVerbose(false); @@ -7220,15 +6594,15 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS { VertexVelocity* VV = new VertexVelocity(pKFi); VV->setId(maxKFid+3*(pKFi->mnId)+1); - VV->setFixed(false); + VV->setFixed(true); optimizer.addVertex(VV); VertexGyroBias* VG = new VertexGyroBias(pKFi); VG->setId(maxKFid+3*(pKFi->mnId)+2); - VG->setFixed(false); + VG->setFixed(true); optimizer.addVertex(VG); VertexAccBias* VA = new VertexAccBias(pKFi); VA->setId(maxKFid+3*(pKFi->mnId)+3); - VA->setFixed(false); + VA->setFixed(true); optimizer.addVertex(VA); } } @@ -7333,7 +6707,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS Verbose::PrintMess("ERROR building inertial edge", Verbose::VERBOSITY_NORMAL); } - Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_NORMAL); + Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_DEBUG); // Set MapPoint vertices @@ -7364,13 +6738,8 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS const float thHuberStereo = sqrt(7.815); const float chi2Stereo2 = 7.815; + const unsigned long iniMPid = maxKFid*5; - - - const unsigned long iniMPid = maxKFid*5; // TODO: should be maxKFid*4; - - - Verbose::PrintMess("start inserting MPs", Verbose::VERBOSITY_NORMAL); for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) { MapPoint* pMP = *lit; @@ -8481,7 +7850,6 @@ void Optimizer::OptimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFram optimizer.addVertex(V4DoF); vpVertices[nIDi]=V4DoF; } - cout << "PoseGraph4DoF: KFs loaded" << endl; set > sInsertedEdges; @@ -8525,7 +7893,6 @@ void Optimizer::OptimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFram sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj))); } } - cout << "PoseGraph4DoF: Loop edges loaded" << endl; // 1. Set normal edges for(size_t i=0, iend=vpKFs.size(); i &vbInliers12, int &nInliers) void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C) { // 矩阵P每一行求和，结果存在C。这两句也可以使用CV_REDUCE_AVG选项来实现 - cv::reduce(P,C,1,CV_REDUCE_SUM); + cv::reduce(P,C,1,cv::REDUCE_SUM); C = C/P.cols;// 求平均 for(int i=0; i(0,0)); // vec/norm(vec)归一化得到归一化后的旋转向量,然后乘上角度得到包含了旋转轴和旋转角信息的旋转向量vec - vec = 2*ang*vec/norm(vec); //Angle-axis x. quaternion angle is the half + vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half mR12i.create(3,3,P1.type()); // 旋转向量（轴角）转换为旋转矩阵 diff --git a/src/System.cc b/src/System.cc index 8cac461..ec7e170 100644 --- a/src/System.cc +++ b/src/System.cc @@ -47,10 +47,10 @@ System::System(const string &strVocFile, //词袋文件所在路 const string &strLoadingFile //看起来作者貌似想加地图重载功能的一个参数 ): mSensor(sensor), //初始化传感器类型 - mpViewer(static_cast(NULL)), // ?空。。。对象指针？ TODO + mpViewer(static_cast(NULL)), // 空对象指针 mbReset(false), mbResetActiveMap(false),// ?重新设置ActiveMap - mbActivateLocalizationMode(false), // ?是否开启局部定位功能开关 - mbDeactivateLocalizationMode(false) // ?没有这个模式转换标志 + mbActivateLocalizationMode(false), // 是否开启局部定位功能开关 + mbDeactivateLocalizationMode(false) // { // Output welcome message cout << endl << @@ -84,7 +84,7 @@ System::System(const string &strVocFile, //词袋文件所在路 exit(-1); } - // ?ORBSLAM3新加的多地图管理功能，这里好像是加载Atlas标识符 + // ORBSLAM3新加的多地图管理功能，这里加载Atlas标识符 bool loadedAtlas = false; //---- @@ -112,75 +112,7 @@ System::System(const string &strVocFile, //词袋文件所在路 //Create the Atlas // Step 5 创建多地图，参数0表示初始化关键帧id为0 mpAtlas = new Atlas(0); - - // 下面注释看起来作者貌似想加地图重载功能，期待期待 - /*if(strLoadingFile.empty()) - { - //Load ORB Vocabulary - cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; - mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); - if(!bVocLoad) - { - cerr << "Wrong path to vocabulary. " << endl; - cerr << "Falied to open at: " << strVocFile << endl; - exit(-1); - } - cout << "Vocabulary loaded!" << endl << endl; - - //Create KeyFrame Database - mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); - - //Create the Atlas - //mpMap = new Map(); - mpAtlas = new Atlas(0); - } - else - { - //Load ORB Vocabulary - cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; - - mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); - if(!bVocLoad) - { - cerr << "Wrong path to vocabulary. " << endl; - cerr << "Falied to open at: " << strVocFile << endl; - exit(-1); - } - cout << "Vocabulary loaded!" << endl << endl; - - cout << "Load File" << endl; - - // Load the file with an earlier session - //clock_t start = clock(); - bool isRead = LoadAtlas(strLoadingFile,BINARY_FILE); - - if(!isRead) - { - cout << "Error to load the file, please try with other session file or vocabulary file" << endl; - exit(-1); - } - mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); - - mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase); - mpAtlas->SetORBVocabulary(mpVocabulary); - mpAtlas->PostLoad(); - //cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl; - - loadedAtlas = true; - - mpAtlas->CreateNewMap(); - - //clock_t timeElapsed = clock() - start; - //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000); - //cout << "Binary file read in " << msElapsed << " ms" << endl; - - //usleep(10*1000*1000); - }*/ - - // 设置Atlas中的传感器类型 if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR) // ? 如果是有imu的传感器类型，将mbIsInertial设置为imu属性,以后的跟踪和预积分将和这个标志有关 mpAtlas->SetInertialSensor(); @@ -203,8 +135,6 @@ System::System(const string &strVocFile, //词袋文件所在路 mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO, strSequence); mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper); - //initFr表示初始化帧的id，代码里设置为0 - mpLocalMapper->mInitFr = initFr; //设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃 mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"]; // ? 这里有个疑问,C++中浮点型跟0比较是否用精确? @@ -302,11 +232,8 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) mpTracker->GrabImuData(vImuMeas[i_imu]); - // std::cout << "start GrabImageStereo" << std::endl; cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp,filename); - // std::cout << "out grabber" << std::endl; - unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; @@ -518,6 +445,10 @@ void System::Shutdown() if(mpViewer) pangolin::BindToContext("ORB-SLAM2: Map Viewer"); + +#ifdef REGISTER_TIMES + mpTracker->PrintTimeStats(); +#endif } @@ -650,7 +581,6 @@ void System::SaveTrajectoryEuRoC(const string &filename) ofstream f; f.open(filename.c_str()); - // cout << "file open" << endl; f << fixed; // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). @@ -663,56 +593,34 @@ void System::SaveTrajectoryEuRoC(const string &filename) list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); - //cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; - //cout << "size mlRelativeFramePoses: " << mpTracker->mlRelativeFramePoses.size() << endl; - //cout << "size mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; - //cout << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; - - for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) { - //cout << "1" << endl; if(*lbL) continue; KeyFrame* pKF = *lRit; - //cout << "KF: " << pKF->mnId << endl; cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); - /*cout << "2" << endl; - cout << "KF id: " << pKF->mnId << endl;*/ - // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. if (!pKF) continue; - //cout << "2.5" << endl; - while(pKF->isBad()) { - //cout << " 2.bad" << endl; Trw = Trw*pKF->mTcp; pKF = pKF->GetParent(); - //cout << "--Parent KF: " << pKF->mnId << endl; } if(!pKF || pKF->GetMap() != pBiggerMap) { - //cout << "--Parent KF is from another map" << endl; - /*if(pKF) - cout << "--Parent KF " << pKF->mnId << " is from another map " << pKF->GetMap()->GetId() << endl;*/ continue; } - //cout << "3" << endl; - Trw = Trw*pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference - // cout << "4" << endl; - if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO) { cv::Mat Tbw = pKF->mImuCalib.Tbc*(*lit)*Trw; @@ -730,7 +638,6 @@ void System::SaveTrajectoryEuRoC(const string &filename) f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) << twc.at(0) << " " << twc.at(1) << " " << twc.at(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; } - // cout << "5" << endl; } //cout << "end saving trajectory" << endl; f.close(); @@ -767,8 +674,6 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) { KeyFrame* pKF = vpKFs[i]; - // pKF->SetPose(pKF->GetPose()*Two); - if(pKF->isBad()) continue; if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO) @@ -843,63 +748,6 @@ void System::SaveTrajectoryKITTI(const string &filename) f.close(); } - -void System::SaveDebugData(const int &initIdx) -{ - // 0. Save initialization trajectory - SaveTrajectoryEuRoC("init_FrameTrajectoy_" +to_string(mpLocalMapper->mInitSect)+ "_" + to_string(initIdx)+".txt"); - - // 1. Save scale - ofstream f; - f.open("init_Scale_" + to_string(mpLocalMapper->mInitSect) + ".txt", ios_base::app); - f << fixed; - f << mpLocalMapper->mScale << endl; - f.close(); - - // 2. Save gravity direction - f.open("init_GDir_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); - f << fixed; - f << mpLocalMapper->mRwg(0,0) << "," << mpLocalMapper->mRwg(0,1) << "," << mpLocalMapper->mRwg(0,2) << endl; - f << mpLocalMapper->mRwg(1,0) << "," << mpLocalMapper->mRwg(1,1) << "," << mpLocalMapper->mRwg(1,2) << endl; - f << mpLocalMapper->mRwg(2,0) << "," << mpLocalMapper->mRwg(2,1) << "," << mpLocalMapper->mRwg(2,2) << endl; - f.close(); - - // 3. Save computational cost - f.open("init_CompCost_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); - f << fixed; - f << mpLocalMapper->mCostTime << endl; - f.close(); - - // 4. Save biases - f.open("init_Biases_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); - f << fixed; - f << mpLocalMapper->mbg(0) << "," << mpLocalMapper->mbg(1) << "," << mpLocalMapper->mbg(2) << endl; - f << mpLocalMapper->mba(0) << "," << mpLocalMapper->mba(1) << "," << mpLocalMapper->mba(2) << endl; - f.close(); - - // 5. Save covariance matrix - f.open("init_CovMatrix_" +to_string(mpLocalMapper->mInitSect)+ "_" +to_string(initIdx)+".txt", ios_base::app); - f << fixed; - for(int i=0; imcovInertial.rows(); i++) - { - for(int j=0; jmcovInertial.cols(); j++) - { - if(j!=0) - f << ","; - f << setprecision(15) << mpLocalMapper->mcovInertial(i,j); - } - f << endl; - } - f.close(); - - // 6. Save initialization time - f.open("init_Time_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app); - f << fixed; - f << mpLocalMapper->mInitTime << endl; - f.close(); -} - - int System::GetTrackingState() { unique_lock lock(mMutexState); @@ -933,7 +781,7 @@ bool System::isLost() return false; else { - if ((mpTracker->mState==Tracking::LOST)) //||(mpTracker->mState==Tracking::RECENTLY_LOST)) + if ((mpTracker->mState==Tracking::LOST)) return true; else return false; @@ -960,154 +808,18 @@ void System::ChangeDataset() mpTracker->NewDataset(); } -/*void System::SaveAtlas(int type){ - cout << endl << "Enter the name of the file if you want to save the current Atlas session. To exit press ENTER: "; - string saveFileName; - getline(cin,saveFileName); - if(!saveFileName.empty()) - { - //clock_t start = clock(); - - // Save the current session - mpAtlas->PreSave(); - mpKeyFrameDatabase->PreSave(); - - string pathSaveFileName = "./"; - pathSaveFileName = pathSaveFileName.append(saveFileName); - pathSaveFileName = pathSaveFileName.append(".osa"); - - string strVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE); - std::size_t found = mStrVocabularyFilePath.find_last_of("/\\"); - string strVocabularyName = mStrVocabularyFilePath.substr(found+1); - - if(type == TEXT_FILE) // File text - { - cout << "Starting to write the save text file " << endl; - std::remove(pathSaveFileName.c_str()); - std::ofstream ofs(pathSaveFileName, std::ios::binary); - boost::archive::text_oarchive oa(ofs); - - oa << strVocabularyName; - oa << strVocabularyChecksum; - oa << mpAtlas; - oa << mpKeyFrameDatabase; - cout << "End to write the save text file" << endl; - } - else if(type == BINARY_FILE) // File binary - { - cout << "Starting to write the save binary file" << endl; - std::remove(pathSaveFileName.c_str()); - std::ofstream ofs(pathSaveFileName, std::ios::binary); - boost::archive::binary_oarchive oa(ofs); - oa << strVocabularyName; - oa << strVocabularyChecksum; - oa << mpAtlas; - oa << mpKeyFrameDatabase; - cout << "End to write save binary file" << endl; - } - - //clock_t timeElapsed = clock() - start; - //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000); - //cout << "Binary file saved in " << msElapsed << " ms" << endl; - } +#ifdef REGISTER_TIMES +void System::InsertRectTime(double& time) +{ + mpTracker->vdRectStereo_ms.push_back(time); } -bool System::LoadAtlas(string filename, int type) +void System::InsertTrackTime(double& time) { - string strFileVoc, strVocChecksum; - bool isRead = false; - - if(type == TEXT_FILE) // File text - { - cout << "Starting to read the save text file " << endl; - std::ifstream ifs(filename, std::ios::binary); - if(!ifs.good()) - { - cout << "Load file not found" << endl; - return false; - } - boost::archive::text_iarchive ia(ifs); - ia >> strFileVoc; - ia >> strVocChecksum; - ia >> mpAtlas; - //ia >> mpKeyFrameDatabase; - cout << "End to load the save text file " << endl; - isRead = true; - } - else if(type == BINARY_FILE) // File binary - { - cout << "Starting to read the save binary file" << endl; - std::ifstream ifs(filename, std::ios::binary); - if(!ifs.good()) - { - cout << "Load file not found" << endl; - return false; - } - boost::archive::binary_iarchive ia(ifs); - ia >> strFileVoc; - ia >> strVocChecksum; - ia >> mpAtlas; - //ia >> mpKeyFrameDatabase; - cout << "End to load the save binary file" << endl; - isRead = true; - } - - if(isRead) - { - //Check if the vocabulary is the same - string strInputVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE); - - if(strInputVocabularyChecksum.compare(strVocChecksum) != 0) - { - cout << "The vocabulary load isn't the same which the load session was created " << endl; - cout << "-Vocabulary name: " << strFileVoc << endl; - return false; // Both are differents - } - - return true; - } - return false; + mpTracker->vdTrackTotal_ms.push_back(time); } +#endif -string System::CalculateCheckSum(string filename, int type) -{ - string checksum = ""; - - unsigned char c[MD5_DIGEST_LENGTH]; - - std::ios_base::openmode flags = std::ios::in; - if(type == BINARY_FILE) // Binary file - flags = std::ios::in | std::ios::binary; - - ifstream f(filename.c_str(), flags); - if ( !f.is_open() ) - { - cout << "[E] Unable to open the in file " << filename << " for Md5 hash." << endl; - return checksum; - } - - MD5_CTX md5Context; - char buffer[1024]; - - MD5_Init (&md5Context); - while ( int count = f.readsome(buffer, sizeof(buffer))) - { - MD5_Update(&md5Context, buffer, count); - } - - f.close(); - - MD5_Final(c, &md5Context ); - - for(int i = 0; i < MD5_DIGEST_LENGTH; i++) - { - char aux[10]; - sprintf(aux,"%02x", c[i]); - checksum = checksum + aux; - } - - return checksum; -}*/ } //namespace ORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 6fb77f5..56cddf5 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -19,21 +19,20 @@ #include "Tracking.h" -#include -#include +#include +#include -#include"ORBmatcher.h" -#include"FrameDrawer.h" -#include"Converter.h" -#include"Initializer.h" -#include"G2oTypes.h" -#include"Optimizer.h" -#include"PnPsolver.h" +#include "ORBmatcher.h" +#include "FrameDrawer.h" +#include "Converter.h" +#include "Initializer.h" +#include "G2oTypes.h" +#include "Optimizer.h" -#include +#include -#include -#include +#include +#include #include #include #include @@ -49,7 +48,7 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false), mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL), - mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), + mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0), time_recently_lost_visual(2.0), mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr) { // Load camera parameters from settings file @@ -85,52 +84,6 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, mbInitWith3KFs = false; - - //Rectification parameters - /*mbNeedRectify = false; - if((mSensor == System::STEREO || mSensor == System::IMU_STEREO) && sCameraName == "PinHole") - { - cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; - fSettings["LEFT.K"] >> K_l; - fSettings["RIGHT.K"] >> K_r; - - fSettings["LEFT.P"] >> P_l; - fSettings["RIGHT.P"] >> P_r; - - fSettings["LEFT.R"] >> R_l; - fSettings["RIGHT.R"] >> R_r; - - fSettings["LEFT.D"] >> D_l; - fSettings["RIGHT.D"] >> D_r; - - int rows_l = fSettings["LEFT.height"]; - int cols_l = fSettings["LEFT.width"]; - int rows_r = fSettings["RIGHT.height"]; - int cols_r = fSettings["RIGHT.width"]; - - if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() - || rows_l==0 || cols_l==0 || rows_r==0 || cols_r==0) - { - mbNeedRectify = false; - } - else - { - mbNeedRectify = true; - // M1r y M2r son los outputs (igual para l) - // M1r y M2r son las matrices relativas al mapeo correspondiente a la rectificación de mapa en el eje X e Y respectivamente - //cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); - //cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); - } - - - } - else - { - int cols = 752; - int rows = 480; - cv::Mat R_l = cv::Mat::eye(3, 3, CV_32F); - }*/ - mnNumDataset = 0; if(!b_parse_cam || !b_parse_orb || !b_parse_imu) @@ -146,24 +99,399 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, } } - //f_track_stats.open("tracking_stats"+ _nameSeq + ".txt"); - /*f_track_stats.open("tracking_stats.txt"); - f_track_stats << "# timestamp, Num KF local, Num MP local, time" << endl; - f_track_stats << fixed ;*/ +#ifdef REGISTER_TIMES + vdRectStereo_ms.clear(); + vdORBExtract_ms.clear(); + vdStereoMatch_ms.clear(); + vdIMUInteg_ms.clear(); + vdPosePred_ms.clear(); + vdLMTrack_ms.clear(); + vdNewKF_ms.clear(); + vdTrackTotal_ms.clear(); -#ifdef SAVE_TIMES - f_track_times.open("tracking_times.txt"); - f_track_times << "# ORB_Ext(ms), Stereo matching(ms), Preintegrate_IMU(ms), Pose pred(ms), LocalMap_track(ms), NewKF_dec(ms)" << endl; - f_track_times << fixed ; + vdUpdatedLM_ms.clear(); + vdSearchLP_ms.clear(); + vdPoseOpt_ms.clear(); #endif + + vnKeyFramesLM.clear(); + vnMapPointsLM.clear(); } +#ifdef REGISTER_TIMES +double calcAverage(vector v_times) +{ + double accum = 0; + for(double value : v_times) + { + accum += value; + } + + return accum / v_times.size(); +} + +double calcDeviation(vector v_times, double average) +{ + double accum = 0; + for(double value : v_times) + { + accum += pow(value - average, 2); + } + return sqrt(accum / v_times.size()); +} + +double calcAverage(vector v_values) +{ + double accum = 0; + int total = 0; + for(double value : v_values) + { + if(value == 0) + continue; + accum += value; + total++; + } + + return accum / total; +} + +double calcDeviation(vector v_values, double average) +{ + double accum = 0; + int total = 0; + for(double value : v_values) + { + if(value == 0) + continue; + accum += pow(value - average, 2); + total++; + } + return sqrt(accum / total); +} + +void Tracking::LocalMapStats2File() +{ + ofstream f; + f.open("LocalMapTimeStats.txt"); + f << fixed << setprecision(6); + f << "#Stereo rect[ms], MP culling[ms], MP creation[ms], LBA[ms], KF culling[ms], Total[ms]" << endl; + for(int i=0; ivdLMTotal_ms.size(); ++i) + { + f << mpLocalMapper->vdKFInsert_ms[i] << "," << mpLocalMapper->vdMPCulling_ms[i] << "," + << mpLocalMapper->vdMPCreation_ms[i] << "," << mpLocalMapper->vdLBA_ms[i] << "," + << mpLocalMapper->vdKFCulling_ms[i] << "," << mpLocalMapper->vdLMTotal_ms[i] << endl; + } + + f.close(); + + f.open("LBA_Stats.txt"); + f << fixed << setprecision(6); + f << "#LBA time[ms], KF opt[#], KF fixed[#], MP[#], Edges[#]" << endl; + for(int i=0; ivdLBASync_ms.size(); ++i) + { + f << mpLocalMapper->vdLBASync_ms[i] << "," << mpLocalMapper->vnLBA_KFopt[i] << "," + << mpLocalMapper->vnLBA_KFfixed[i] << "," << mpLocalMapper->vnLBA_MPs[i] << "," + << mpLocalMapper->vnLBA_edges[i] << endl; + } + + + f.close(); +} + +void Tracking::TrackStats2File() +{ + ofstream f; + f.open("SessionInfo.txt"); + f << fixed; + f << "Number of KFs: " << mpAtlas->GetAllKeyFrames().size() << endl; + f << "Number of MPs: " << mpAtlas->GetAllMapPoints().size() << endl; + + f << "OpenCV version: " << CV_VERSION << endl; + + f.close(); + + f.open("TrackingTimeStats.txt"); + f << fixed << setprecision(6); + + f << "#KF insert[ms], ORB ext[ms], Stereo match[ms], IMU preint[ms], Pose pred[ms], LM track[ms], KF dec[ms], Total[ms]" << endl; + + for(int i=0; ivdKFInsert_ms); + deviation = calcDeviation(mpLocalMapper->vdKFInsert_ms, average); + std::cout << "KF Insertion: " << average << "$\\pm$" << deviation << std::endl; + f << "KF Insertion: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdMPCulling_ms); + deviation = calcDeviation(mpLocalMapper->vdMPCulling_ms, average); + std::cout << "MP Culling: " << average << "$\\pm$" << deviation << std::endl; + f << "MP Culling: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdMPCreation_ms); + deviation = calcDeviation(mpLocalMapper->vdMPCreation_ms, average); + std::cout << "MP Creation: " << average << "$\\pm$" << deviation << std::endl; + f << "MP Creation: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdLBASync_ms); + deviation = calcDeviation(mpLocalMapper->vdLBASync_ms, average); + std::cout << "LBA: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdKFCullingSync_ms); + deviation = calcDeviation(mpLocalMapper->vdKFCullingSync_ms, average); + std::cout << "KF Culling: " << average << "$\\pm$" << deviation << std::endl; + f << "KF Culling: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vdLMTotal_ms); + deviation = calcDeviation(mpLocalMapper->vdLMTotal_ms, average); + std::cout << "Total Local Mapping: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Local Mapping: " << average << "$\\pm$" << deviation << std::endl; + + // Local Mapping LBA complexity + std::cout << "---------------------------" << std::endl; + std::cout << std::endl << "LBA complexity (mean$\\pm$std)" << std::endl; + f << "---------------------------" << std::endl; + f << std::endl << "LBA complexity (mean$\\pm$std)" << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_edges); + deviation = calcDeviation(mpLocalMapper->vnLBA_edges, average); + std::cout << "LBA Edges: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA Edges: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_KFopt); + deviation = calcDeviation(mpLocalMapper->vnLBA_KFopt, average); + std::cout << "LBA KF optimized: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA KF optimized: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_KFfixed); + deviation = calcDeviation(mpLocalMapper->vnLBA_KFfixed, average); + std::cout << "LBA KF fixed: " << average << "$\\pm$" << deviation << std::endl; + f << "LBA KF fixed: " << average << "$\\pm$" << deviation << std::endl; + + average = calcAverage(mpLocalMapper->vnLBA_MPs); + deviation = calcDeviation(mpLocalMapper->vnLBA_MPs, average); + std::cout << "LBA MP: " << average << "$\\pm$" << deviation << std::endl << std::endl; + f << "LBA MP: " << average << "$\\pm$" << deviation << std::endl << std::endl; + + std::cout << "LBA executions: " << mpLocalMapper->nLBA_exec << std::endl; + std::cout << "LBA aborts: " << mpLocalMapper->nLBA_abort << std::endl; + f << "LBA executions: " << mpLocalMapper->nLBA_exec << std::endl; + f << "LBA aborts: " << mpLocalMapper->nLBA_abort << std::endl; + + // Map complexity + std::cout << "---------------------------" << std::endl; + std::cout << std::endl << "Map complexity" << std::endl; + std::cout << "KFs in map: " << mpAtlas->GetAllMaps()[0]->GetAllKeyFrames().size() << std::endl; + std::cout << "MPs in map: " << mpAtlas->GetAllMaps()[0]->GetAllMapPoints().size() << std::endl; + f << "---------------------------" << std::endl; + f << std::endl << "Map complexity" << std::endl; + f << "KFs in map: " << mpAtlas->GetAllMaps()[0]->GetAllKeyFrames().size() << std::endl; + f << "MPs in map: " << mpAtlas->GetAllMaps()[0]->GetAllMapPoints().size() << std::endl; + + + // Place recognition time stats + std::cout << std::endl << std::endl << std::endl; + std::cout << "Place Recognition (mean$\\pm$std)" << std::endl << std::endl; + f << std::endl << "Place Recognition (mean$\\pm$std)" << std::endl << std::endl; + + average = calcAverage(mpLoopClosing->vTimeBoW_ms); + deviation = calcDeviation(mpLoopClosing->vTimeBoW_ms, average); + std::cout << "Database Query: " << average << "$\\pm$" << deviation << std::endl; + f << "Database Query: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vTimeSE3_ms); + deviation = calcDeviation(mpLoopClosing->vTimeSE3_ms, average); + std::cout << "SE3 estimation: " << average << "$\\pm$" << deviation << std::endl; + f << "SE3 estimation: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vTimePRTotal_ms); + deviation = calcDeviation(mpLoopClosing->vTimePRTotal_ms, average); + std::cout << "Total Place Recognition: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Place Recognition: " << average << "$\\pm$" << deviation << std::endl; + + // Loop Closing time stats + if(mpLoopClosing->vTimeLoopTotal_ms.size() > 0) + { + std::cout << std::endl << std::endl << std::endl; + std::cout << "Loop Closing (mean$\\pm$std)" << std::endl << std::endl; + f << std::endl << "Loop Closing (mean$\\pm$std)" << std::endl << std::endl; + + average = calcAverage(mpLoopClosing->vTimeLoopTotal_ms); + deviation = calcDeviation(mpLoopClosing->vTimeLoopTotal_ms, average); + std::cout << "Total Loop Closing: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Loop Closing: " << average << "$\\pm$" << deviation << std::endl; + } + + if(mpLoopClosing->vTimeMergeTotal_ms.size() > 0) + { + // Map Merging time stats + std::cout << std::endl << std::endl << std::endl; + std::cout << "Map Merging (mean$\\pm$std)" << std::endl << std::endl; + f << std::endl << "Map Merging (mean$\\pm$std)" << std::endl << std::endl; + + average = calcAverage(mpLoopClosing->vTimeMergeTotal_ms); + deviation = calcDeviation(mpLoopClosing->vTimeMergeTotal_ms, average); + std::cout << "Total Map Merging: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Map Merging: " << average << "$\\pm$" << deviation << std::endl; + } + + + if(mpLoopClosing->vTimeGBATotal_ms.size() > 0) + { + // Full GBA time stats + std::cout << std::endl << std::endl << std::endl; + std::cout << "Full GBA (mean$\\pm$std)" << std::endl << std::endl; + f << std::endl << "Full GBA (mean$\\pm$std)" << std::endl << std::endl; + + average = calcAverage(mpLoopClosing->vTimeFullGBA_ms); + deviation = calcDeviation(mpLoopClosing->vTimeFullGBA_ms, average); + std::cout << "GBA: " << average << "$\\pm$" << deviation << std::endl; + f << "GBA: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vTimeMapUpdate_ms); + deviation = calcDeviation(mpLoopClosing->vTimeMapUpdate_ms, average); + std::cout << "Map Update: " << average << "$\\pm$" << deviation << std::endl; + f << "Map Update: " << average << "$\\pm$" << deviation << std::endl; + average = calcAverage(mpLoopClosing->vTimeGBATotal_ms); + deviation = calcDeviation(mpLoopClosing->vTimeGBATotal_ms, average); + std::cout << "Total Full GBA: " << average << "$\\pm$" << deviation << std::endl; + f << "Total Full GBA: " << average << "$\\pm$" << deviation << std::endl; + } + + f.close(); + +} + +#endif + Tracking::~Tracking() { - //f_track_stats.close(); -#ifdef SAVE_TIMES - f_track_times.close(); -#endif + } bool Tracking::ParseCamParamFile(cv::FileStorage &fSettings) @@ -918,26 +1246,26 @@ cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRe { if(mbRGB) { - cvtColor(mImGray,mImGray,CV_RGB2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_RGB2GRAY); } else { - cvtColor(mImGray,mImGray,CV_BGR2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_BGR2GRAY); } } else if(mImGray.channels()==4) { if(mbRGB) { - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_RGBA2GRAY); } else { - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); + cvtColor(imGrayRight,imGrayRight,cv::COLOR_BGRA2GRAY); } } @@ -950,32 +1278,16 @@ cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRe else if(mSensor == System::IMU_STEREO && mpCamera2) mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera,mpCamera2,mTlr,&mLastFrame,*mpImuCalib); - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); mCurrentFrame.mNameFile = filename; mCurrentFrame.mnDataset = mnNumDataset; - Track(); - - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); - - double t_track = std::chrono::duration_cast >(t1 - t0).count(); - - /*cout << "trracking time: " << t_track << endl; - f_track_stats << setprecision(0) << mCurrentFrame.mTimeStamp*1e9 << ","; - f_track_stats << mvpLocalKeyFrames.size() << ","; - f_track_stats << mvpLocalMapPoints.size() << ","; - f_track_stats << setprecision(6) << t_track << endl;*/ - -#ifdef SAVE_TIMES - f_track_times << mCurrentFrame.mTimeORB_Ext << ","; - f_track_times << mCurrentFrame.mTimeStereoMatch << ","; - f_track_times << mTime_PreIntIMU << ","; - f_track_times << mTime_PosePred << ","; - f_track_times << mTime_LocalMapTrack << ","; - f_track_times << mTime_NewKF_Dec << ","; - f_track_times << t_track << endl; +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); + vdStereoMatch_ms.push_back(mCurrentFrame.mTimeStereoMatch); #endif + Track(); + return mCurrentFrame.mTcw.clone(); } @@ -988,49 +1300,32 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d if(mImGray.channels()==3) { if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGB2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); else - cvtColor(mImGray,mImGray,CV_BGR2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); } else if(mImGray.channels()==4) { if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); else - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); } if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F) imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor); - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth,mpCamera); mCurrentFrame.mNameFile = filename; mCurrentFrame.mnDataset = mnNumDataset; +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); +#endif Track(); - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); - - double t_track = std::chrono::duration_cast >(t1 - t0).count(); - - /*f_track_stats << setprecision(0) << mCurrentFrame.mTimeStamp*1e9 << ","; - f_track_stats << mvpLocalKeyFrames.size() << ","; - f_track_stats << mvpLocalMapPoints.size() << ","; - f_track_stats << setprecision(6) << t_track << endl;*/ - -#ifdef SAVE_TIMES - f_track_times << mCurrentFrame.mTimeORB_Ext << ","; - f_track_times << mCurrentFrame.mTimeStereoMatch << ","; - f_track_times << mTime_PreIntIMU << ","; - f_track_times << mTime_PosePred << ","; - f_track_times << mTime_LocalMapTrack << ","; - f_track_times << mTime_NewKF_Dec << ","; - f_track_times << t_track << endl; -#endif - return mCurrentFrame.mTcw.clone(); } @@ -1051,16 +1346,16 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, if(mImGray.channels()==3) { if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGB2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY); else - cvtColor(mImGray,mImGray,CV_BGR2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGR2GRAY); } else if(mImGray.channels()==4) { if(mbRGB) - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_RGBA2GRAY); else - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); + cvtColor(mImGray,mImGray,cv::COLOR_BGRA2GRAY); } // Step 2 ：构造Frame @@ -1086,36 +1381,18 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, if (mState==NO_IMAGES_YET) t0=timestamp; - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); - mCurrentFrame.mNameFile = filename; mCurrentFrame.mnDataset = mnNumDataset; +#ifdef REGISTER_TIMES + vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); +#endif + lastID = mCurrentFrame.mnId; // Step 3 ：跟踪 Track(); - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); - - double t_track = std::chrono::duration_cast >(t1 - t0).count(); - - /*f_track_stats << setprecision(0) << mCurrentFrame.mTimeStamp*1e9 << ","; - f_track_stats << mvpLocalKeyFrames.size() << ","; - f_track_stats << mvpLocalMapPoints.size() << ","; - f_track_stats << setprecision(6) << t_track << endl;*/ - -//开启保存数据模式 -#ifdef SAVE_TIMES - f_track_times << mCurrentFrame.mTimeORB_Ext << ","; - f_track_times << mCurrentFrame.mTimeStereoMatch << ","; - f_track_times << mTime_PreIntIMU << ","; - f_track_times << mTime_PosePred << ","; - f_track_times << mTime_LocalMapTrack << ","; - f_track_times << mTime_NewKF_Dec << ","; - f_track_times << t_track << endl; -#endif - //返回当前帧的位姿 return mCurrentFrame.mTcw.clone(); } @@ -1454,10 +1731,6 @@ void Tracking::ComputeVelocitiesAccBias(const vector &vpFs, float &bax, } } -void Tracking::ResetFrameIMU() -{ - // TODO To implement... -} /** * @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪 @@ -1469,13 +1742,7 @@ void Tracking::ResetFrameIMU() */ void Tracking::Track() { -#ifdef SAVE_TIMES - mTime_PreIntIMU = 0; - mTime_PosePred = 0; - mTime_LocalMapTrack = 0; - mTime_NewKF_Dec = 0; -#endif - // ? 还不清楚这个标识符,跟绘图相关? + if (bStepByStep) { while(!mbStep) @@ -1556,15 +1823,16 @@ void Tracking::Track() // Step 4 IMU模式下对IMU数据进行预积分 -> // ? 没有创建地图的情况下 if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap) { -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now(); #endif // IMU数据进行预积分 PreintegrateIMU(); -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now(); - mTime_PreIntIMU = std::chrono::duration_cast >(t1 - t0).count(); + double timePreImu = std::chrono::duration_cast >(time_EndPreIMU - time_StartPreIMU).count(); + vdIMUInteg_ms.push_back(timePreImu); #endif @@ -1625,14 +1893,15 @@ void Tracking::Track() // Step 6 系统成功初始化，下面是具体跟踪过程 bool bOK; +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now(); +#endif + // Initial camera pose estimation using motion model or relocalization (if tracking is lost) // mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式 // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking if(!mbOnlyTracking) { -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point timeStartPosePredict = std::chrono::steady_clock::now(); -#endif // State OK // Local Mapping is activated. This is the normal behaviour, unless @@ -1761,13 +2030,6 @@ void Tracking::Track() } } - -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point timeEndPosePredict = std::chrono::steady_clock::now(); - - mTime_PosePred = std::chrono::duration_cast >(timeEndPosePredict - timeStartPosePredict).count(); -#endif - } else // 纯定位模式，时间关系暂时不看 TOSEE { @@ -1871,7 +2133,18 @@ void Tracking::Track() if(!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; - // If we have an initial estimation of the camera pose and matching. Track the local map. +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now(); + + double timePosePred = std::chrono::duration_cast >(time_EndPosePred - time_StartPosePred).count(); + vdPosePred_ms.push_back(timePosePred); +#endif + + +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now(); +#endif + // Step 7 在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化 // 在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿 @@ -1883,17 +2156,8 @@ void Tracking::Track() { if(bOK) { -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point time_StartTrackLocalMap = std::chrono::steady_clock::now(); -#endif // 局部地图跟踪 bOK = TrackLocalMap(); -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point time_EndTrackLocalMap = std::chrono::steady_clock::now(); - - mTime_LocalMapTrack = std::chrono::duration_cast >(time_EndTrackLocalMap - time_StartTrackLocalMap).count(); -#endif - } if(!bOK) @@ -1978,13 +2242,19 @@ void Tracking::Track() if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU)) { cout << "RESETING FRAME!!!" << endl; - ResetFrameIMU(); } else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30)) mLastBias = mCurrentFrame.mImuBias; // 没啥用，后面会重新赋值后传给普通帧 } } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now(); + + double timeLMTrack = std::chrono::duration_cast >(time_EndLMTrack - time_StartLMTrack).count(); + vdLMTrack_ms.push_back(timeLMTrack); +#endif + // Update drawer // 更新显示线程中的图像、特征点、地图点等信息 mpFrameDrawer->Update(this); @@ -2051,16 +2321,12 @@ void Tracking::Track() // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露 mlpTemporalPoints.clear(); -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point timeStartNewKF = std::chrono::steady_clock::now(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now(); #endif // 判断是否需要插入关键帧 bool bNeedKF = NeedNewKeyFrame(); -#ifdef SAVE_TIMES - std::chrono::steady_clock::time_point timeEndNewKF = std::chrono::steady_clock::now(); - mTime_NewKF_Dec = std::chrono::duration_cast >(timeEndNewKF - timeStartNewKF).count(); -#endif @@ -2073,6 +2339,13 @@ void Tracking::Track() // 创建关键帧，对于双目或RGB-D会产生新的地图点 CreateNewKeyFrame(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now(); + + double timeNewKF = std::chrono::duration_cast >(time_EndNewKF - time_StartNewKF).count(); + vdNewKF_ms.push_back(timeNewKF); +#endif + // We allow points with high innovation (considererd outliers by the Huber Function) // pass to the new keyframe, so that bundle adjustment will finally decide // if they are outliers or not. We don't want next frame to estimate its position @@ -2343,8 +2616,6 @@ void Tracking::MonocularInitialization() CreateInitialMapMonocular(); - // Just for video - // bStepByStep = true; } } } @@ -2581,7 +2852,6 @@ bool Tracking::TrackReferenceKeyFrame() int nmatchesMap = 0; for(int i =0; i= mCurrentFrame.Nleft) break; if(mCurrentFrame.mvpMapPoints[i]) { if(mCurrentFrame.mvbOutlier[i]) @@ -2781,8 +3051,24 @@ bool Tracking::TrackLocalMap() // We retrieve the local map and try to find matches to points in the local map. mTrackedFr++; +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartLMUpdate = std::chrono::steady_clock::now(); +#endif UpdateLocalMap(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartSearchLP = std::chrono::steady_clock::now(); + + double timeUpdatedLM_ms = std::chrono::duration_cast >(time_StartSearchLP - time_StartLMUpdate).count(); + vdUpdatedLM_ms.push_back(timeUpdatedLM_ms); +#endif + SearchLocalPoints(); +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_StartPoseOpt = std::chrono::steady_clock::now(); + + double timeSearchLP_ms = std::chrono::duration_cast >(time_StartPoseOpt - time_StartSearchLP).count(); + vdSearchLP_ms.push_back(timeSearchLP_ms); +#endif // TOO check outliers before PO int aux1 = 0, aux2=0; @@ -2806,8 +3092,7 @@ bool Tracking::TrackLocalMap() } else { - // if(!mbMapUpdated && mState == OK) // && (mnMatchesInliers>30)) - if(!mbMapUpdated) // && (mnMatchesInliers>30)) + if(!mbMapUpdated) { Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); @@ -2819,6 +3104,15 @@ bool Tracking::TrackLocalMap() } } } +#ifdef REGISTER_TIMES + std::chrono::steady_clock::time_point time_EndPoseOpt = std::chrono::steady_clock::now(); + + double timePoseOpt_ms = std::chrono::duration_cast >(time_EndPoseOpt - time_StartPoseOpt).count(); + vdPoseOpt_ms.push_back(timePoseOpt_ms); +#endif + + vnKeyFramesLM.push_back(mvpLocalKeyFrames.size()); + vnMapPointsLM.push_back(mvpLocalMapPoints.size()); aux1 = 0, aux2 = 0; for(int i=0; imThDepth && nPoints>maxPoint) @@ -3226,7 +3520,7 @@ void Tracking::SearchLocalPoints() th=5; if(mState==LOST || mState==RECENTLY_LOST) // Lost for less than 1 second - th=15; // 15 + th=15; int matches = matcher.SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th, mpLocalMapper->mbFarPoints, mpLocalMapper->mThFarPoints); } @@ -3314,7 +3608,6 @@ void Tracking::UpdateLocalKeyFrames() } else { - // MODIFICATION mLastFrame.mvpMapPoints[i]=NULL; } } @@ -3350,7 +3643,7 @@ void Tracking::UpdateLocalKeyFrames() for(vector::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++) { // Limit the number of keyframes - if(mvpLocalKeyFrames.size()>80) // 80 + if(mvpLocalKeyFrames.size()>80) break; KeyFrame* pKF = *itKF; @@ -3747,12 +4040,9 @@ void Tracking::ResetActiveMap(bool bLocMap) // Clear Map (this erase MapPoints and KeyFrames) mpAtlas->clearMap(); - - //KeyFrame::nNextId = mpAtlas->GetLastInitKFid(); - //Frame::nNextId = mnLastInitFrameId; mnLastInitFrameId = Frame::nNextId; mnLastRelocFrameId = mnLastInitFrameId; - mState = NO_IMAGES_YET; //NOT_INITIALIZED; + mState = NO_IMAGES_YET; if(mpInitializer) { @@ -3761,7 +4051,6 @@ void Tracking::ResetActiveMap(bool bLocMap) } list lbLost; - // lbLost.reserve(mlbLost.size()); unsigned int index = mnFirstFrameId; cout << "mnFirstFrameId = " << mnFirstFrameId << endl; for(Map* pMap : mpAtlas->GetAllMaps()) @@ -3773,7 +4062,6 @@ void Tracking::ResetActiveMap(bool bLocMap) } } - //cout << "First Frame id: " << index << endl; int num_lost = 0; cout << "mnInitialFrameId = " << mnInitialFrameId << endl; diff --git a/src/Viewer.cc b/src/Viewer.cc index 6ac81e1..bfc0805 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -145,15 +145,12 @@ void Viewer::Run() pangolin::Var menuFollowCamera("menu.Follow Camera",true,true); pangolin::Var menuCamView("menu.Camera View",false,false); pangolin::Var menuTopView("menu.Top View",false,false); - // pangolin::Var menuSideView("menu.Side View",false,false); pangolin::Var menuShowPoints("menu.Show Points",true,true); pangolin::Var menuShowKeyFrames("menu.Show KeyFrames",true,true); pangolin::Var menuShowGraph("menu.Show Graph",false,true); pangolin::Var menuShowInertialGraph("menu.Show Inertial Graph",true,true); pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true); pangolin::Var menuReset("menu.Reset",false,false); - pangolin::Var menuStepByStep("menu.Step By Step",false,true); // false, true - pangolin::Var menuStep("menu.Step",false,false); // Define Camera Render Object (for view / scene browsing) pangolin::OpenGlRenderState s_cam( @@ -192,7 +189,6 @@ void Viewer::Run() if(mbStopTrack) { - menuStepByStep = true; mbStopTrack = false; } @@ -237,21 +233,11 @@ void Viewer::Run() { menuTopView = false; bCameraView = false; - /*s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000)); - s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));*/ s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0)); s_cam.Follow(Ow); } - /*if(menuSideView && mpMapDrawer->mpAtlas->isImuInitialized()) - { - s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000)); - s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0.0,0.1,30.0,0,0,0,0.0,0.0,1.0)); - s_cam.Follow(Twwp); - }*/ - - if(menuLocalizationMode && !bLocalizationMode) { mpSystem->ActivateLocalizationMode(); @@ -263,24 +249,6 @@ void Viewer::Run() bLocalizationMode = false; } - if(menuStepByStep && !bStepByStep) - { - mpTracker->SetStepByStep(true); - bStepByStep = true; - } - else if(!menuStepByStep && bStepByStep) - { - mpTracker->SetStepByStep(false); - bStepByStep = false; - } - - if(menuStep) - { - mpTracker->mbStep = true; - menuStep = false; - } - - d_cam.Activate(s_cam); glClearColor(1.0f,1.0f,1.0f,1.0f); mpMapDrawer->DrawCurrentCamera(Twc); @@ -317,7 +285,6 @@ void Viewer::Run() bLocalizationMode = false; bFollow = true; menuFollowCamera = true; - //mpSystem->Reset(); mpSystem->ResetActiveMap(); menuReset = false; }